public PathRequest(Vector3 _start,
Vector3 _end,
float _maxLength,
PathfinderImplementationStrategy aStarImpl)
{
pathStart = _start;
pathEnd = _end;
maxLength = _maxLength;
this.aStarImpl = aStarImpl;
}
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
));
}
