#pragma warning restore 649 public void FindPath(PathRequest request, Action <PathResult> callback) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; PathNode startNode = _grid.NodeFromWorldPoint(request.pathStart); PathNode targetNode = _grid.NodeFromWorldPoint(request.pathEnd); if (startNode.Walkable && targetNode.Walkable) { Heap <PathNode> openSet = new Heap <PathNode>(_grid.MaxSize); HashSet <PathNode> closedSet = new HashSet <PathNode>(); openSet.Add(startNode); while (openSet.Count > 0) { PathNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { /*sw.Stop(); * UnityEngine.Debug.Log("Path found in " + sw.Elapsed);*/ pathSuccess = true; break; } foreach (PathNode neighbour in _grid.GetNeighbours(currentNode)) { if (!neighbour.Walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.GCost + Mathf.RoundToInt(GetDistance(currentNode, neighbour) * neighbour.MovementPenalty); if (newMovementCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour)) { neighbour.GCost = newMovementCostToNeighbour; neighbour.HCost = GetDistance(neighbour, targetNode); neighbour.Parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); pathSuccess = waypoints.Length > 0; } callback(new PathResult(waypoints, pathSuccess, request.callback)); } }
public void RequestPath(PathRequest request) { ThreadStart threadStart = delegate { _pathfinder.FindPath(request, FinishProcessingPath); }; threadStart.Invoke(); }