IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] wayPoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodefromWorldPosition(startPos); Node targetNode = grid.NodefromWorldPosition(targetPos); if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { // 結束條件 pathSuccess = true; sw.Stop(); //print("Cost: " + sw.ElapsedMilliseconds + "ms"); break; } foreach (Node neighbor in grid.getNeighbours(currentNode)) { if (!neighbor.walkable || closedSet.Contains(neighbor)) { continue; } int newMovementCost_toNeighbor = currentNode.gCost + getDistance(currentNode, neighbor) + neighbor.movementPenalty; if (newMovementCost_toNeighbor < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = newMovementCost_toNeighbor; neighbor.hCost = getDistance(neighbor, targetNode); neighbor.pareent = currentNode; if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } else { openSet.UpdateItem(neighbor); } } } } } yield return(null); if (pathSuccess) { wayPoints = RetracePath(startNode, targetNode); } pathRequestManager.FinshedProcessingPath(wayPoints, pathSuccess); }
IEnumerator FindPath(Vector3 StartPosition, Vector3 TargetPosition) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] Waypoints = new Vector3[0]; bool IsPathSuccess = false; StarNode StartNode = Gird.GetNodeFromWorldPoint(StartPosition); StarNode TargetNode = Gird.GetNodeFromWorldPoint(TargetPosition); if (StartNode.IsWalkable && TargetNode.IsWalkable) { Heap <StarNode> OpenSet = new Heap <StarNode>(Gird.MaxSize); HashSet <StarNode> ClosedSet = new HashSet <StarNode>(); OpenSet.Add(StartNode); while (OpenSet.Count > 0) { StarNode CurrentNode = OpenSet.RemoveFirst(); ClosedSet.Add(CurrentNode); if (CurrentNode == TargetNode) { sw.Stop(); print("Path found: " + sw.ElapsedMilliseconds + " ML"); IsPathSuccess = true; break; } foreach (StarNode Neighbor in Gird.GetNeighbors(CurrentNode)) { if (!Neighbor.IsWalkable || ClosedSet.Contains(Neighbor)) { continue; } int NewMovementCostToNeighour = CurrentNode.Gcost + GetDistance(CurrentNode, Neighbor); if (NewMovementCostToNeighour < Neighbor.Gcost || !OpenSet.Contains(Neighbor)) { Neighbor.Gcost = NewMovementCostToNeighour; Neighbor.Hcost = GetDistance(Neighbor, TargetNode); Neighbor.NodeParent = CurrentNode; if (!OpenSet.Contains(Neighbor)) { OpenSet.Add(Neighbor); OpenSet.UpdateItem(Neighbor); } } } } } yield return(null); if (IsPathSuccess) { Waypoints = RetracePath(StartNode, TargetNode); } RequestManager.FinshedProcessingPath(Waypoints, IsPathSuccess); }