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