public void StepThroughPath()
{
// Verify at least TWO END POINTS ARE SET!
if (this.selectedPathPoints.Count != 2)
{
return;
}
JPSState.state = eJPSState.ST_FIND_PATH;
JPSState.LastPathFound = true;
if (findPath == null)
{
BlockScript[] points = this.selectedPathPoints.ToArray();
Point start = points[0].nodeReference.pos;
Point stop = points[1].nodeReference.pos;
// Get enumerator path finding
findPath = grid.getPathAsync(start, stop);
findPath.MoveNext(); // First iteration doesn't really do anything, so just skip it
}
// step through path finding process
while (findPath.MoveNext())
{
PathfindReturn curr_return = (PathfindReturn)findPath.Current;
switch (curr_return._status)
{
case PathfindReturn.PathfindStatus.SEARCHING:
// render path up to this point
List <Point> path_so_far = grid.reconstructPath(
curr_return._current,
selectedPathPoints.Peek().nodeReference.pos
);
_pathRenderer.drawPath(path_so_far);
break;
case PathfindReturn.PathfindStatus.FOUND:
// render path
_pathRenderer.drawPath(curr_return.path);
findPath = null;
JPSState.state = eJPSState.ST_PATH_FIND_COMPLETE;
return;
case PathfindReturn.PathfindStatus.NOT_FOUND:
// disable rendering, ya blew it
_pathRenderer.disablePath();
findPath = null;
JPSState.state = eJPSState.ST_PATH_FIND_COMPLETE;
JPSState.LastPathFound = false; // tell everyone that we failed to find a path
return;
}
}
Debug.Log("WE ARRIVED AT THE END!");
findPath = null;
JPSState.state = eJPSState.ST_PATH_FIND_COMPLETE;
}
public IEnumerator getPathAsync(Point start, Point goal, List <Point> list)
{
PriorityQueue <PathfindingNode, int> open_set = new PriorityQueue <PathfindingNode, int>();
bool found_path = false;
PathfindReturn return_status = new PathfindReturn();
ResetPathfindingNodeData();
PathfindingNode starting_node = this.pathfindingNodes[pointToIndex(start)];
starting_node.pos = start;
starting_node.parent = null;
starting_node.givenCost = 0;
starting_node.finalCost = 0;
starting_node.listStatus = ListStatus.ON_OPEN;
open_set.push(starting_node, 0);
while (!open_set.isEmpty())
{
PathfindingNode curr_node = open_set.pop();
PathfindingNode parent = curr_node.parent;
Node jp_node = gridNodes[pointToIndex(curr_node.pos)]; // get jump point info
return_status._current = curr_node;
// Check if we've reached the goal
if (curr_node.pos.Equals(goal))
{
// end and return path
return_status.path = reconstructPath(curr_node, start, list);
return_status._status = PathfindReturn.PathfindStatus.FOUND;
found_path = true;
yield return(return_status);
break;
}
yield return(return_status);
// foreach direction from parent
foreach (eDirections dir in getAllValidDirections(curr_node))
{
PathfindingNode new_successor = null;
int given_cost = 0;
// goal is closer than wall distance or closer than or equal to jump point distnace
if (isCardinal(dir) &&
goalIsInExactDirection(curr_node.pos, dir, goal) &&
Point.diff(curr_node.pos, goal) <= Mathf.Abs(jp_node.jpDistances[(int)dir]))
{
new_successor = this.pathfindingNodes[pointToIndex(goal)];
given_cost = curr_node.givenCost + Point.diff(curr_node.pos, goal);
}
// Goal is closer or equal in either row or column than wall or jump point distance
else if (isDiagonal(dir) &&
goalIsInGeneralDirection(curr_node.pos, dir, goal) &&
(Mathf.Abs(goal.column - curr_node.pos.column) <= Mathf.Abs(jp_node.jpDistances[(int)dir]) ||
Mathf.Abs(goal.row - curr_node.pos.row) <= Mathf.Abs(jp_node.jpDistances[(int)dir])))
{
// Create a target jump point
// int minDiff = min(RowDiff(curNode, goalNode),
// ColDiff(curNode, goalNode));
int min_diff = Mathf.Min(Mathf.Abs(goal.column - curr_node.pos.column),
Mathf.Abs(goal.row - curr_node.pos.row));
// newSuccessor = GetNode (curNode, minDiff, direction);
new_successor = getNodeDist(
curr_node.pos.row,
curr_node.pos.column,
dir,
min_diff);
// givenCost = curNode->givenCost + (SQRT2 * DiffNodes(curNode, newSuccessor));
given_cost = curr_node.givenCost + (SQRT_2 * Point.diff(curr_node.pos, new_successor.pos)).ToInt();
var origi = curr_node.givenCost + (Mathf.Sqrt(2) * Point.diff(curr_node.pos, new_successor.pos));
Debug.Log(origi + " " + given_cost);
}
else if (jp_node.jpDistances[(int)dir] > 0)
{
// Jump Point in this direction
// newSuccessor = GetNode(curNode, direction);
new_successor = getNodeDist(
curr_node.pos.row,
curr_node.pos.column,
dir,
jp_node.jpDistances[(int)dir]);
// givenCost = DiffNodes(curNode, newSuccessor);
given_cost = Point.diff(curr_node.pos, new_successor.pos);
// if (diagonal direction) { givenCost *= SQRT2; }
if (isDiagonal(dir))
{
given_cost = (given_cost * SQRT_2).ToInt();
}
// givenCost += curNode->givenCost;
given_cost += curr_node.givenCost;
}
// Traditional A* from this point
if (new_successor != null)
{
// if (newSuccessor not on OpenList)
if (new_successor.listStatus != ListStatus.ON_OPEN)
{
// newSuccessor->parent = curNode;
new_successor.parent = curr_node;
// newSuccessor->givenCost = givenCost;
new_successor.givenCost = given_cost;
new_successor.directionFromParent = dir;
// newSuccessor->finalCost = givenCost +
// CalculateHeuristic(curNode, goalNode);
new_successor.finalCost = given_cost + octileHeuristic(new_successor.pos.column, new_successor.pos.row, goal.column, goal.row);
new_successor.listStatus = ListStatus.ON_OPEN;
// OpenList.Push(newSuccessor);
open_set.push(new_successor, new_successor.finalCost);
}
// else if(givenCost < newSuccessor->givenCost)
else if (given_cost < new_successor.givenCost)
{
// newSuccessor->parent = curNode;
new_successor.parent = curr_node;
// newSuccessor->givenCost = givenCost;
new_successor.givenCost = given_cost;
new_successor.directionFromParent = dir;
// newSuccessor->finalCost = givenCost +
// CalculateHeuristic(curNode, goalNode);
new_successor.finalCost = given_cost + octileHeuristic(new_successor.pos.column, new_successor.pos.row, goal.column, goal.row);
new_successor.listStatus = ListStatus.ON_OPEN;
// OpenList.Update(newSuccessor);
open_set.push(new_successor, new_successor.finalCost);
}
}
}
}
if (!found_path)
{
return_status._status = PathfindReturn.PathfindStatus.NOT_FOUND;
yield return(return_status);
}
}