/// <summary> /// Gets the neighbors nodes. /// </summary> /// <returns> /// The neighbors. /// </returns> /// <param name='currentNode'> /// Current node. /// </param> /// <param name='destinationNode'> /// Destination node. /// </param> private List<Node> GetNeighbors(Node currentNode, Vector3 destinationNode) { mNeighbors = new List<Node> (); Node parentNode = currentNode.ParentNode; bool condition; if (parentNode != null) { int dx = Mathf.Min (Mathf.Max (-1, currentNode.X - parentNode.X), 1); int dy = Mathf.Min (Mathf.Max (-1, currentNode.Y - parentNode.Y), 1); // diagonal expanding if (dx != 0 && dy != 0) { condition = isReachable (currentNode.X, currentNode.Y, currentNode.X, currentNode.Y + dy); checkAndCreateNeighbor (currentNode, 0, dy, destinationNode, condition); condition = isReachable (currentNode.X, currentNode.Y, currentNode.X + dx, currentNode.Y); checkAndCreateNeighbor (currentNode, dx, 0, destinationNode, condition); condition = (mNeighbors.Count > 0 && isReachable (currentNode.X, currentNode.Y, currentNode.X + dx, currentNode.Y + dy)); checkAndCreateNeighbor (currentNode, dx, dy, destinationNode, condition); bool tempCondition1 = isReachable (currentNode.X, currentNode.Y, currentNode.X - dx, currentNode.Y); bool tempCondition2 = isReachable (currentNode.X, currentNode.Y, currentNode.X, currentNode.Y + dy); bool tempCondition3 = isReachable (currentNode.X, currentNode.Y, currentNode.X - dx, currentNode.Y - dy); condition = tempCondition1 == false && tempCondition2 && tempCondition3; checkAndCreateNeighbor (currentNode, -dx, -dy, destinationNode, condition); tempCondition1 = isReachable (currentNode.X, currentNode.Y, currentNode.X, currentNode.Y - dy); tempCondition2 = isReachable (currentNode.X, currentNode.Y, currentNode.X + dx, currentNode.Y); tempCondition3 = isReachable (currentNode.X, currentNode.Y, currentNode.X + dx, currentNode.Y - dy); condition = tempCondition1 == false && tempCondition2 && tempCondition3; checkAndCreateNeighbor (currentNode, dx, -dy, destinationNode, condition); } else if (dx == 0) { // (vertical || horizontal) expanding if (isReachable (currentNode.X, currentNode.Y, currentNode.X, currentNode.Y + dy)) { condition = isReachable (currentNode.X, currentNode.Y, currentNode.X, currentNode.Y + dy); checkAndCreateNeighbor (currentNode, 0, dy, destinationNode, condition); bool tempCondition1 = isReachable (currentNode.X, currentNode.Y, currentNode.X + 1, currentNode.Y); bool tempCondition2 = isReachable (currentNode.X, currentNode.Y, currentNode.X + 1, currentNode.Y + dy); condition = tempCondition1 == false && tempCondition2; checkAndCreateNeighbor (currentNode, 1, dy, destinationNode, condition); tempCondition1 = isReachable (currentNode.X, currentNode.Y, currentNode.X - 1, currentNode.Y); tempCondition2 = isReachable (currentNode.X, currentNode.Y, currentNode.X - 1, currentNode.Y + dy); condition = tempCondition1 == false && tempCondition2; checkAndCreateNeighbor (currentNode, -1, dy, destinationNode, condition); } } else { if (isReachable (currentNode.X, currentNode.Y, currentNode.X + dx, currentNode.Y)) { condition = isReachable (currentNode.X, currentNode.Y, currentNode.X + dx, currentNode.Y); checkAndCreateNeighbor (currentNode, dx, 0, destinationNode, condition); bool tempCondition1 = isReachable (currentNode.X, currentNode.Y, currentNode.X, currentNode.Y + 1); bool tempCondition2 = isReachable (currentNode.X, currentNode.Y, currentNode.X + dx, currentNode.Y + 1); condition = tempCondition1 == false && tempCondition2; checkAndCreateNeighbor (currentNode, dx, 1, destinationNode, condition); tempCondition1 = isReachable (currentNode.X, currentNode.Y, currentNode.X, currentNode.Y - 1); tempCondition2 = isReachable (currentNode.X, currentNode.Y, currentNode.X + dx, currentNode.Y - 1); condition = tempCondition1 == false && tempCondition2; checkAndCreateNeighbor (currentNode, dx, -1, destinationNode, condition); } } } else { // top check // x x x // - - - // - - - condition = isReachable (currentNode.X, currentNode.Y, currentNode.X - 1, currentNode.Y + 1); checkAndCreateNeighbor (currentNode, -1, 1, destinationNode, condition); condition = isReachable (currentNode.X, currentNode.Y, currentNode.X, currentNode.Y + 1); checkAndCreateNeighbor (currentNode, 0, 1, destinationNode, condition); condition = isReachable (currentNode.X, currentNode.Y, currentNode.X + 1, currentNode.Y + 1); checkAndCreateNeighbor (currentNode, 1, 1, destinationNode, condition); // center check // - - - // x - x // - - - condition = isReachable (currentNode.X, currentNode.Y, currentNode.X + 1, currentNode.Y); checkAndCreateNeighbor (currentNode, 1, 0, destinationNode, condition); condition = isReachable (currentNode.X, currentNode.Y, currentNode.X - 1, currentNode.Y); checkAndCreateNeighbor (currentNode, -1, 0, destinationNode, condition); // bottom check // - - - // - - - // x x x condition = isReachable (currentNode.X, currentNode.Y, currentNode.X - 1, currentNode.Y - 1); checkAndCreateNeighbor (currentNode, -1, -1, destinationNode, condition); condition = isReachable (currentNode.X, currentNode.Y, currentNode.X, currentNode.Y - 1); checkAndCreateNeighbor (currentNode, 0, -1, destinationNode, condition); condition = isReachable (currentNode.X, currentNode.Y, currentNode.X + 1, currentNode.Y - 1); checkAndCreateNeighbor (currentNode, 1, -1, destinationNode, condition); } return mNeighbors; }
/// <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)); } }
/// <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); ; } mX = x; mY = y; mNodePosition = nodePosition; mIsDestinationNode = (mNodePosition == destinationPosition); mHeuristicEstimateCost = Node.estimate (nodePosition, destinationPosition); // f = g + h mCostFunction = mCostFromStart + mHeuristicEstimateCost; }
/// <summary> /// Checks the and create neighbor. /// </summary> /// <param name='currentNode'> /// Current node. /// </param> /// <param name='dx'> /// Dx. /// </param> /// <param name='dy'> /// Dy. /// </param> /// <param name='destinationTarget'> /// Destination target. /// </param> /// <param name='condition'> /// Condition. /// </param> private void checkAndCreateNeighbor(Node currentNode, int dx, int dy, Vector3 destinationTarget, bool condition) { if (condition) { Vector3 worldPosition = GetWorldPosition (currentNode.X + dx, currentNode.Y + dy); Node neighbor = new Node (currentNode, currentNode.X + dx, currentNode.Y + dy, worldPosition, destinationTarget); if (neighbor.Equals (currentNode.ParentNode) == false) { mNeighbors.Add (neighbor); } } }
/// <summary> /// Determines whether the specified <see cref="Node"/> is equal to the current <see cref="JumpPointSearach.Node"/>. /// </summary> /// <param name='item'> /// The <see cref="Node"/> to compare with the current <see cref="JumpPointSearach.Node"/>. /// </param> /// <returns> /// <c>true</c> if the specified <see cref="Node"/> is equal to the current <see cref="JumpPointSearach.Node"/>; /// otherwise, <c>false</c>. /// </returns> public bool Equals(Node item) { if (item == null) return false; return (mX == item.X && mY == item.Y); }