/// <summary> /// Initializes a new instance of the <see cref="JumpPointSearach.Node"/> class. /// </summary> /// <param name='parentNode'> /// Parent node. /// </param> /// <param name='x'> /// X. /// </param> /// <param name='y'> /// Y. /// </param> /// <param name='nodePosition'> /// Node position. /// </param> /// <param name='destinationPosition'> /// Destination position. /// </param> public Node(Node parentNode, int x, int y, Vector3?nodePosition, Vector3?destinationPosition) { if (parentNode != null) { mParentNode = parentNode; mCostFromStart = mParentNode.CostFromStart + Node.estimate(mParentNode.NodePosition, nodePosition.Value); ; } mX = x; mY = y; mNodePosition = nodePosition.Value; mIsDestinationNode = (mNodePosition == destinationPosition.Value); mHeuristicEstimateCost = Node.estimate(nodePosition.Value, destinationPosition.Value); // f = g + h mCostFunction = mCostFromStart + mHeuristicEstimateCost; }
/// <summary> /// Finds the path. /// </summary> /// <returns> /// The path. /// </returns> /// <param name='start'> /// Start vector. /// </param> /// <param name='destinationTarget'> /// Destination target. /// </param> private IEnumerator FindPath(Vector3 start, Vector3 destinationTarget) { foreach (Object path in mPaths) { Destroy(path); } mPaths.Clear(); List <Vector3> paths = new List <Vector3> (); SortedHeap <Node> openList = new SortedHeap <Node> (new NodeComparer()); SortedHeap <Node> closedList = new SortedHeap <Node> (new NodeComparer()); Point startPoint = GetGridPosition(start); Point destinationPoint = GetGridPosition(destinationTarget); if (startPoint.X == destinationPoint.X && startPoint.Y == destinationPoint.Y) { yield break; } Vector3 worldStart = GetWorldPosition(startPoint.X, startPoint.Y); Vector3 worldDestination = GetWorldPosition(destinationPoint.X, destinationPoint.Y); openList.Push(new Node(null, startPoint.X, startPoint.Y, worldStart, worldDestination)); Node currentNode = null; Node jumpNode = null; Point jumpPoint = null; int index = 0; float cost = 0; float time = Time.realtimeSinceStartup; while (openList.Count > 0) { currentNode = openList.Pop(); if (currentNode.IsDestinationNode) { while (currentNode != null) { paths.Add(currentNode.NodePosition); currentNode = currentNode.ParentNode; } break; } foreach (Node neighbor in (currentNode.Neighbors == null) ? (currentNode.Neighbors = GetNeighbors(currentNode, worldDestination)) : currentNode.Neighbors) { jumpPoint = Jump(neighbor.X, neighbor.Y, currentNode.X, currentNode.Y, destinationTarget); if (jumpPoint == null) { continue; } jumpNode = new Node(currentNode, jumpPoint.X, jumpPoint.Y, GetWorldPosition(jumpPoint.X, jumpPoint.Y), destinationTarget); if (closedList.Contains(jumpNode)) { continue; } float estimatedCost = Node.estimate(jumpNode.NodePosition, currentNode.NodePosition); cost = currentNode.CostFromStart + estimatedCost; if (openList.Contains(jumpNode, ref index) && cost >= jumpNode.CostFromStart) { continue; } jumpNode.ParentNode = currentNode; jumpNode.CostFromStart = cost; jumpNode.CostFunction = jumpNode.CostFromStart + jumpNode.HeuristicEstimateCost; if (index >= 0) { openList.RemoveAt(index); } openList.Push(neighbor); } closedList.Push(currentNode); } Debug.Log(string.Format(timeAndNodesFormat, (Time.realtimeSinceStartup - time) * 1000f, paths.Count)); //Debug.Log (string.Format ("Nodes in path: {0}", paths.Count)); foreach (Vector3 path in paths) { mPaths.Add(Instantiate(Gizmo, path + mGizmoHeight * 2, Quaternion.identity)); } }