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 )); }