/// <summary>
/// Initializes a new instance of the <see cref="PathingAStar"/> class.
/// </summary>
/// <param name="heapInitialSize">Initial size of the heap.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="pathSmoother">The path smoother to use.</param>
public PathingAStar(int heapInitialSize, IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths pathSmoother)
: base(moveCostProvider, cellCostStrategy, pathSmoother)
{
_openSet = new BinaryHeap<IPathNode>(heapInitialSize, new PathNodeComparer());
_expandedSet = new List<IPathNode>();
_successorArray = new DynamicArray<IPathNode>(15);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingAStar"/> class.
/// </summary>
/// <param name="heapInitialSize">Initial size of the heap.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="cellCostStrategy">The cell cost provider.</param>
/// <param name="pathSmoother">The path smoother to use.</param>
/// <param name="requestPreprocessors">The list of request preprocessors to use.</param>
public PathingAStar(int heapInitialSize, IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths pathSmoother, IRequestPreProcessor[] requestPreprocessors)
: base(moveCostProvider, cellCostStrategy, pathSmoother, requestPreprocessors)
{
_openSet = new BinaryHeap <IPathNode>(heapInitialSize, new PathNodeComparer());
_expandedSet = new List <IPathNode>();
_successorArray = new DynamicArray <IPathNode>(15);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingEngineBase"/> class.
/// </summary>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="smoother">The path smoother to use</param>
protected PathingEngineBase(IMoveCost moveCostProvider, ISmoothPaths smoother)
{
Ensure.ArgumentNotNull(moveCostProvider, "moveCostProvider");
Ensure.ArgumentNotNull(smoother, "smoother");
_costProvider = moveCostProvider;
_smoother = smoother;
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingEngineBase"/> class.
/// </summary>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="smoother">The path smoother to use</param>
protected PathingEngineBase(IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths smoother)
{
Ensure.ArgumentNotNull(moveCostProvider, "moveCostProvider");
Ensure.ArgumentNotNull(cellCostStrategy, "cellCostStrategy");
Ensure.ArgumentNotNull(smoother, "smoother");
_costProvider = moveCostProvider;
_smoother = smoother;
_cellCostStrategy = cellCostStrategy;
_coroutineIter = new SafeIterator(this);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingEngineBase"/> class.
/// </summary>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="smoother">The path smoother to use</param>
protected PathingEngineBase(IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths smoother)
{
Ensure.ArgumentNotNull(moveCostProvider, "moveCostProvider");
Ensure.ArgumentNotNull(cellCostStrategy, "cellCostStrategy");
Ensure.ArgumentNotNull(smoother, "smoother");
_costProvider = moveCostProvider;
_smoother = smoother;
_cellCostStrategy = cellCostStrategy;
_coroutineIter = new SafeIterator(this);
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns>
/// The cost
/// </returns>
public int GetActionCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
//This is kind of an arbitrary thing. Under normal circumstances the cost of moving from one cell to another is equal to the distance between the cells.
//Based on that the cost here would be the length of the arc of the jump. However speed is also a factor. One could get a reference to the IDefine speed component and use the current speed setting,
//but we want to use a different speed for jumps, so to be accurate we would need to determine the speed difference and take that into account.
//We do not want this calculation to be overly complex however, so for this example we keep it simple by using assumptions. This will produce a less accurate cost but...
var halfDistance = (to.position - from.position).magnitude / 2.0f;
var hmax = this.heightFactor * halfDistance * 2.0f;
var assumedNormalSpeed = 3.0f;
return((int)(2 * Mathf.Sqrt((halfDistance * halfDistance) + (hmax * hmax)) / (this.groundSpeed / assumedNormalSpeed)) * costProvider.baseMoveCost);
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingEngineBase"/> class.
/// </summary>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="cellCostStrategy">The cell cost provider.</param>
/// <param name="smoother">The path smoother to use</param>
/// <param name="requestPreprocessors">The list of request preprocessors to use.</param>
protected PathingEngineBase(IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths smoother, IRequestPreProcessor[] requestPreprocessors)
{
Ensure.ArgumentNotNull(moveCostProvider, "moveCostProvider");
Ensure.ArgumentNotNull(cellCostStrategy, "cellCostStrategy");
Ensure.ArgumentNotNull(smoother, "smoother");
_costProvider = moveCostProvider;
_smoother = smoother;
_cellCostStrategy = cellCostStrategy;
_coroutineIter = new SafeIterator(this);
_segmentRequest = new SegmentRequest();
_segments = new DynamicArray <Path>(10);
_requestPreprocessors = requestPreprocessors;
}
/// <summary>
/// Gets the heuristic for a node in relation to this portal. This is only used for Shortcut portals, but is valid for all types.
/// </summary>
/// <param name="node">The node.</param>
/// <param name="goal">The goal.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <returns>The heuristic</returns>
public int GetHeuristic(IPathNode node, IPathNode goal, IMoveCost moveCostProvider)
{
//The logic here is that we want to shortest possible distance from the node to this portal cell,
//combined with the shortest possible distance from our partner portal cell to the goal, again combined with the
// cost of making the portal move using those two entry points.
int mx = _matrixBounds.AdjustColumnToBounds(node.matrixPosX);
int mz = _matrixBounds.AdjustRowToBounds(node.matrixPosZ);
var closestCellToNode = this.parent.rawMatrix[mx, mz];
mx = _partner._matrixBounds.AdjustColumnToBounds(goal.matrixPosX);
mz = _partner._matrixBounds.AdjustRowToBounds(goal.matrixPosZ);
var closestCellToGoal = _partner.parent.rawMatrix[mx, mz];
return(moveCostProvider.GetHeuristic(node, closestCellToNode) + _action.GetActionCost(closestCellToNode, closestCellToGoal, moveCostProvider) + moveCostProvider.GetHeuristic(closestCellToGoal, goal));
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingJumpPointSearch"/> class.
/// </summary>
/// <param name="heapInitialSize">Initial size of the heap.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="pathSmoother">The path smoother to use.</param>
public PathingJumpPointSearch(int heapInitialSize, IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths pathSmoother)
: base(heapInitialSize, moveCostProvider, cellCostStrategy, pathSmoother)
{
_neighbours = new DynamicArray<IPathNode>(8);
}
/// <summary>
/// Gets the heuristic for a node in relation to this portal.
/// </summary>
/// <param name="node">The node.</param>
/// <param name="goal">The goal.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <returns>The heuristic</returns>
public int GetHeuristic(IPathNode node, IPathNode goal, IMoveCost moveCostProvider)
{
return moveCostProvider.GetHeuristic(node, this) + moveCostProvider.GetHeuristic(_partner, goal);
}
/// <summary>
/// Dijkstra's algorithm, shortest paths between nodes in a graph
/// https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm
/// Computes the distance table between two nodes in a graph
/// All positions have 8 degrees of freedom, and the cost of moving between positions are equal to
/// 1 + the absolute difference in value.
/// </summary>
/// <param name="items">2D value array</param>
/// <param name="initial">Start position</param>
/// <param name="end">End positions</param>
public static int[,] Dijkstra(IMoveCost[,] items, Position initial, Position end)
{
// Let the node at which we are starting be called the initial node. Let the distance of node Y be the distance from the initial node to Y.
// Dijkstra's algorithm will assign some initial distance values and will try to improve them step by step.
int W = items.GetLength(0);
int H = items.GetLength(1);
int[,] distance = new int[W, H];
bool[,] visited = new bool[W, H];
// 1. Assign to every node a tentative distance value: set it to zero for our initial node and to infinity for all other nodes.
// 2. Set the initial node as current. Mark all other nodes unvisited. Create a set of all the unvisited nodes called the unvisited set.
List<Position> unvisited = new List<Position>();
for (int x = 0; x < W; x++)
for (int y = 0; y < H; y++)
{
distance[x, y] = int.MaxValue;
visited[x, y] = false;
if (!(initial.x == x && initial.y == y))
unvisited.Add(new Position(x,y));
}
distance[initial.x, initial.y] = 0;
Position current = initial;
while (true) {
// For the current node, consider all of its unvisited neighbors and calculate their tentative distances.
// Compare the newly calculated tentative distance to the current assigned value and assign the smaller one.
// For example, if the current node A is marked with a distance of 6, and the edge connecting it with a neighbor B
// has length 2, then the distance to B (through A) will be 6 + 2 = 8. If B was previously marked with a distance
// greater than 8 then change it to 8. Otherwise, keep the current value.
var neighbors = GetUnvisitedNeighbors(visited, current);
foreach (Position neighbor in neighbors)
{
// get the cost of moving to the neighbor
int cost = items[neighbor.x, neighbor.y].MoveCost(items[current.x, current.y]);
// if the path is shorter than a previous path to the neighbor, update it
int path = MOVE_PENALTY + cost + distance[current.x, current.y];
if (path < distance[neighbor.x, neighbor.y])
distance[neighbor.x, neighbor.y] = path;
}
// When we are done considering all of the neighbors of the current node, mark the current node as visited
// and remove it from the unvisited set. A visited node will never be checked again.
unvisited.Remove(current);
visited[current.x, current.y] = true;
// If the destination node has been marked visited (when planning a route between two specific nodes)
// or if the smallest tentative distance among the nodes in the unvisited set is infinity
// (when planning a complete traversal; occurs when there is no connection between the initial node and
// remaining unvisited nodes), then stop. The algorithm has finished.
if (visited[end.x, end.y])
break;
// Otherwise, select the unvisited node that is marked with the smallest tentative distance,
// set it as the new "current node", and go back to step 3.
current = (from p in unvisited
orderby distance[p.x, p.y] ascending
select new Position(p.x, p.y)).First();
}
return distance;
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns></returns>
public int GetActionCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return 0;
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns>The cost</returns>
public int GetCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return _action.GetActionCost(from, to, costProvider);
}
/// <summary>
/// Gets the heuristic for a node in relation to this portal. This is only used for Shortcut portals, but is valid for all types.
/// </summary>
/// <param name="node">The node.</param>
/// <param name="goal">The goal.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <returns>The heuristic</returns>
public int GetHeuristic(IPathNode node, IPathNode goal, IMoveCost moveCostProvider)
{
//The logic here is that we want to shortest possible distance from the node to this portal cell,
//combined with the shortest possible distance from our partner portal cell to the goal, again combined with the
// cost of making the portal move using those two entry points.
int mx = _matrixBounds.AdjustColumnToBounds(node.matrixPosX);
int mz = _matrixBounds.AdjustRowToBounds(node.matrixPosZ);
var closestCellToNode = this.parent.rawMatrix[mx, mz];
mx = _partner._matrixBounds.AdjustColumnToBounds(goal.matrixPosX);
mz = _partner._matrixBounds.AdjustRowToBounds(goal.matrixPosZ);
var closestCellToGoal = _partner.parent.rawMatrix[mx, mz];
return moveCostProvider.GetHeuristic(node, closestCellToNode) + _action.GetActionCost(closestCellToNode, closestCellToGoal, moveCostProvider) + moveCostProvider.GetHeuristic(closestCellToGoal, goal);
}
/// <summary>
/// Computes the shortest path between two positions, in a 2D table.
/// All positions have 8 degrees of freedom, and the cost of moving between positions are equal to
/// 1 + the absolute difference in value.
/// </summary>
/// <param name="values">2D Cell array</param>
/// <param name="initial">Start position</param>
/// <param name="end">End positions</param>
/// <returns></returns>
public static List<Position> GetShortestPath(IMoveCost[,] values, Position initial, Position end)
{
// Use Dijkstra's algorithm to compute the distance table for the given values
int[,] dist = Dijkstra(values, initial, end);
// traverse the shortest path and return list
return GetShortestPathFromDistance(dist, initial, end);
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns></returns>
public int GetActionCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return(0);
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns></returns>
public int GetActionCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return costProvider.GetHeuristic(from, to);
}
public VisualizedJPS(int heapInitialSize, IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths pathSmoother, IRequestPreProcessor[] requestPreprocessors)
: base(heapInitialSize, moveCostProvider, cellCostStrategy, pathSmoother, requestPreprocessors)
{
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns></returns>
public int GetActionCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return(costProvider.GetHeuristic(from, to));
}
/// <summary>
/// Calculates the cost of moving between two IMoveCost objects
/// </summary>
/// <param name="B">Object to move to</param>
/// <returns>Cost</returns>
public int MoveCost(IMoveCost B)
{
return(Math.Abs(this.Value - B.Value));
}
/// <summary>
/// Gets the heuristic from this portal to the goal.
/// </summary>
/// <param name="goal">The goal.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <returns>The heuristic</returns>
//TODO: this si not used, so remove it
public int GetHeuristic(IPathNode goal, IMoveCost moveCostProvider)
{
return(moveCostProvider.GetHeuristic(_partner, goal));
}
/// <summary>
/// Initializes a new instance of the <see cref="PathingJumpPointSearch"/> class.
/// </summary>
/// <param name="heapInitialSize">Initial size of the heap.</param>
/// <param name="moveCostProvider">The move cost provider.</param>
/// <param name="cellCostStrategy">The cell cost provider.</param>
/// <param name="pathSmoother">The path smoother to use.</param>
/// <param name="requestPreprocessors">The list of request preprocessors to use.</param>
public PathingJumpPointSearch(int heapInitialSize, IMoveCost moveCostProvider, ICellCostStrategy cellCostStrategy, ISmoothPaths pathSmoother, IRequestPreProcessor[] requestPreprocessors)
: base(heapInitialSize, moveCostProvider, cellCostStrategy, pathSmoother, requestPreprocessors)
{
_neighbours = new DynamicArray <IPathNode>(8);
}
/// <summary>
/// Gets the action cost.
/// </summary>
/// <param name="from">The node from which the action will start.</param>
/// <param name="to">The node at which the action will end.</param>
/// <param name="costProvider">The cost provider in use by the path finder.</param>
/// <returns>The cost</returns>
public int GetCost(IPositioned from, IPositioned to, IMoveCost costProvider)
{
return(_action.GetActionCost(from, to, costProvider));
}
/// <summary>
/// Calculates the cost of moving between two IMoveCost objects
/// </summary>
/// <param name="B">Object to move to</param>
/// <returns>Cost</returns>
public int MoveCost(IMoveCost B)
{
return Math.Abs(this.Value - B.Value);
}
