private void Awake()
{
visitedNodes = new List <PathfinderNode>();
nodesToDraw = new List <PathfinderNode>();
instance = this;
}
public static PathResult FindPath(
PathRequest request
)
{
instance.openSet.Clear();
instance.activeNodes.Clear();
instance.closedSet.Clear();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
float maxLength = request.maxLength;
PathfinderImplementationStrategy implementationStrategy = request.aStarImpl;
int maxPathLength = instance.grid.DistanceToNodeDistance(maxLength);
Point startPoint = instance.grid.WorldCoordToNode(request.pathStart);
Point endPoint = instance.grid.WorldCoordToNode(request.pathEnd);
PathfinderNode startNode =
implementationStrategy.CreateStarterNodes(
startPoint,
instance.grid.GetMapNodeAt(startPoint)
);
PathfinderNode targetNode =
implementationStrategy.CreateStarterNodes(
endPoint,
instance.grid.GetMapNodeAt(endPoint)
);
/*
* If startnode and targetnode are the same
* it causes a heap error
*/
if (startNode.IsWalkable() && targetNode.IsWalkable())
{
instance.openSet.Add(startNode);
instance.activeNodes.Add(startNode.GetGridCoord(), startNode);
instance.activeNodes.Add(targetNode.GetGridCoord(), targetNode);
while (instance.openSet.Count > 0)
{
Profiler.BeginSample("Remove first from open set");
PathfinderNode currentNode = instance.openSet.RemoveFirst();
Profiler.EndSample();
Profiler.BeginSample("Add current to closed set");
instance.closedSet.Add(currentNode);
Profiler.EndSample();
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
if (instance.openSet.Contains(targetNode))
{
pathSuccess = true;
break;
}
Profiler.BeginSample("Process current node");
implementationStrategy.ProcessNode(
currentNode,
startNode,
targetNode,
instance.openSet,
instance.closedSet,
instance.activeNodes,
instance.grid,
maxPathLength
);
Profiler.EndSample();
}
}
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
pathSuccess = waypoints.Length > 0;
}
List <Point> recycledPoints = Pools.ListPoints;
//recycledPoints.Add(startPoint);
//recycledPoints.Add(endPoint);
Dictionary <Point, PathfinderNode> .Enumerator enumerator =
instance.activeNodes.GetEnumerator();
while (enumerator.MoveNext())
{
var current = enumerator.Current;
Point p = current.Key;
recycledPoints.Add(p);
}
for (int i = 0; i < recycledPoints.Count; i++)
{
Point current = recycledPoints[i];
Pools.Point = current;
}
Pools.ListPoints = recycledPoints;
PathfinderVisualizer.Visualize();
//Debug.Log("path end");
return(new PathResult(
waypoints,
pathSuccess
));
}
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
{
}
}
}
