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