/// <summary>
/// Find a path using the AStar algorithm
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="map"></param>
/// <returns></returns>
public static Position <short>[] FindPath(Position <short> start, Position <short> end, IMap map)
{
MiniHeap <Node> delNodes = new MiniHeap <Node>();
MiniHeap <Node> nodes = new MiniHeap <Node>();
nodes.Add(new Node {
Position = start
}, false);
while (nodes.Count() > 0)
{
Node node = nodes.Pop();
if (node.Position.Equals(end))
{
int i = node.Distance;
Position <short>[] path = new Position <short> [i];
while (i >= 1)
{
path[--i] = node.Position;
node = node.Parent;
}
return(path);
}
node.Closed = true;
foreach (Position <short> n in PathfinderHelper.GetNeighbors(node.Position, map))
{
if (n == null)
{
break;
}
Node neighbor = nodes.Get(no => no.Position.Equals(n)) ?? delNodes.Get(no => no.Position.Equals(n));
if (neighbor == null)
{
neighbor = new Node
{
Position = n,
G = node.G + (n.X == node.Position.X || n.Y == node.Position.Y ? 1 : 1.4),
H = Heuristics.Manhattan(Math.Abs(n.X - end.Y), Math.Abs(n.Y - end.Y)),
Parent = node,
Distance = node.Distance + 1
};
neighbor.F = neighbor.G + neighbor.H;
nodes.Add(neighbor);
}
else if (!neighbor.Closed)
{
double gScore = node.G + (n.X == node.Position.X || n.Y == node.Position.Y ? 1 : 1.4);
if (gScore > neighbor.G)
{
continue;
}
neighbor.G = gScore;
neighbor.F = neighbor.G + neighbor.H;
neighbor.Parent = node;
neighbor.Distance = node.Distance + 1;
}
}
delNodes.Add(node, false);
}
return(null);
}
/// <summary> /// Returns an Array with the neighbors of the given position /// </summary> /// <param name="pos"></param> /// <param name="map"></param> /// <returns></returns> public Position <short>[] GetNeighbors(Position <short> pos, IMap map) => PathfinderHelper.GetNeighbors(pos, map);
public override void ProcessNode(
PathfinderNode currentNode,
PathfinderNode startNode,
PathfinderNode targetNode,
PathfindingHeap <PathfinderNode> openSet,
HashSet <PathfinderNode> closedSet,
Dictionary <Point, PathfinderNode> activeNodes,
Grid grid,
int maxPathLength
)
{
Vector3 currentLocation = grid.NodeToWorldCoord(currentNode.GetGridCoord());
List <PathfinderNode> neighbors = PathfinderHelper.GetNeighbors(
currentNode,
activeNodes,
currentNodeCreator
);
foreach (PathfinderNode neighbour in neighbors)
{
Vector3 neighbourLocation = grid.NodeToWorldCoord(neighbour.GetGridCoord());
if (!neighbour.IsWalkable() ||
closedSet.Contains(neighbour))
{
continue;
}
CostResult newStrategyCost =
currentCostStrategy.GetAdditionalCostAt(
currentLocation,
neighbourLocation
);
//neighbour.UpdateAccumulatedStrategyCost(newStrategyCost);
int newPhysicalGCost =
currentNode.GetPhysicalGCost()
+ PathfinderHelper.GetDistance(currentNode, neighbour);
int newStrategyGCost =
currentNode.GetStrategyGCost()
+ neighbour.GetExtractor().Extract(newStrategyCost);
int newMovementCostToNeighbour =
newPhysicalGCost + newStrategyGCost;
// bool smaller = newStrategyGCost < neighbour.GetStrategyGCost();
//if (smaller)
// {
//DrawGizmo.AddGizmo(Color.green, newStrategyGCost + " " + neighbour.GetStrategyGCost(), neighbour.GetLocation());
//}
//Debug.Log(neighbour.GetGCost());
if (newMovementCostToNeighbour < neighbour.GetGCost() || !openSet.Contains(neighbour))
{
//Debug.Log(neighbour.GetGCost());
//DrawGizmo.AddGizmo(Color.green, "" + currentNode.GetExtractor().Extract(newStrategyCost), neighbour.GetLocation());
neighbour.SetStrategyCost(
newStrategyCost
);
neighbour.SetStrategyGCost(
newStrategyGCost
);
neighbour.SetPhysicalGCost(newPhysicalGCost);
neighbour.SetHCost(GetDistance(neighbour, targetNode));
neighbour.SetParent(currentNode);
if (!openSet.Contains(neighbour) &&
neighbour.WithInRangeOfStart(maxPathLength)
)
{
openSet.Add(neighbour);
PathfinderVisualizer.Visit(neighbour);
/*DrawGizmo.AddGizmo(Color.grey, "", grid.NodeToWorldCoord(
* neighbour.GetGridCoord())
* );*/
}
else
{
openSet.UpdateItem(neighbour);
}
}
else
{
}
}
}