private bool CalculateNodes(RouteNode start, RouteNode goal)
{
mGoal = goal;
//Set up the movement costs and heuristic (manhattan distance)
start.MovementCost = 0;
start.Heuristic = GetHeuristic(start);
start.TotalCost = start.Heuristic; //Since movement cost is 0
//Add the start node to the open list
mOpenList.Push(start);
int movementcost = mGrain * mModifier;
//Keep looping until goal is found or there are no more open nodes
while (mOpenList.Count > 0)
{
//Remove from open list
RouteNode parent = GetNextNode(); // pops node off open list based on cost and generation
//Check to see if we have found the goal node
//if (parent.Near(goal, mGrain))
if (parent.Equals(goal))
{
mSolution = parent;
return(true);
}
//Close the node
parent.Closed = true;
//Add or check four adjacent squares to the open list
AddAdjacentNode(mGrain, 0, movementcost, parent);
AddAdjacentNode(0, mGrain, movementcost, parent);
AddAdjacentNode(0, -mGrain, movementcost, parent);
AddAdjacentNode(-mGrain, 0, movementcost, parent);
}
return(false);
}
//Creates the solution from the calculated nodes as vectors
public ArrayList GetSolution()
{
///Add an additional node to the end if the solution didnt match the goal
if (mSolution.Equals(mGoal) || mSolution.Parent == null)
{
mGoal = mSolution;
}
else
{
//If in line then add the goal as a node to the solution, else move the solution node
if (mSolution.X == mGoal.X || mSolution.Y == mGoal.Y)
{
mGoal.Parent = mSolution;
}
else
{
RouteNode extra = new RouteNode();
extra.Parent = mSolution;
//Set node coordinates
extra.X = (mSolution.X == mSolution.Parent.X) ? mSolution.X : mGoal.X;
extra.Y = (mSolution.Y == mSolution.Parent.Y) ? mSolution.Y : mGoal.Y;
//Link the goal to the new node and add it
mGoal.Parent = extra;
}
}
ArrayList list = new ArrayList();
RouteNode previous = mGoal;
RouteNode node = mGoal.Parent;
list.Add(new PointF(previous.X, previous.Y));
//Only one or two items
if (node == null || node.Parent == null)
{
return(list);
}
while (node.Parent != null)
{
if (!((previous.X == node.Parent.X) || (previous.Y == node.Parent.Y)))
{
list.Insert(0, new PointF(node.X, node.Y));
previous = node;
}
node = node.Parent;
}
//Add the start node
PointF start = new PointF(node.X, node.Y);
if (!start.Equals((PointF)list[0]))
{
list.Insert(0, start);
}
return(list);
}
private bool CalculateNodes(RouteNode start, RouteNode goal)
{
_goal = goal;
//Set up the movement costs and heuristic
start.MovementCost = 0;
start.Heuristic = GetHeuristic(start);
start.TotalCost = start.Heuristic; //Since movement cost is 0
start.Direction = NodeDirection.Down; //Set manually to down
//Add the start node to the movement list
//_openList.Push(start);
_movementList.Push(start);
//Keep looping until goal is found or there are no more open nodes
while (_movementList.Count > 0 || _openList.Count > 0)
{
//Remove from open list
RouteNode parent = PopNode(); // pops node off open list based on total cost
//Check to see if we have found the goal node
//if (parent.Near(goal, mGrain))
if (parent.Equals(goal))
{
_solution = parent;
return(true);
}
//Close the node
parent.Closed = true;
//Add or check four adjacent squares to the open list
//The nodes that are added last will pop off first off the binary heap
if (parent.Direction == NodeDirection.Down)
{
AddAdjacentNode(0, -_grain, _grain, parent); //up
AddAdjacentNode(-_grain, 0, _grain, parent); //left
AddAdjacentNode(_grain, 0, _grain, parent); //right
AddAdjacentNode(0, _grain, _grain, parent); //down
}
else if (parent.Direction == NodeDirection.Right)
{
AddAdjacentNode(-_grain, 0, _grain, parent); //left
AddAdjacentNode(0, -_grain, _grain, parent); //up
AddAdjacentNode(0, _grain, _grain, parent); //down
AddAdjacentNode(_grain, 0, _grain, parent); //right
}
else if (parent.Direction == NodeDirection.Left)
{
AddAdjacentNode(_grain, 0, _grain, parent); //right
AddAdjacentNode(0, -_grain, _grain, parent); //up
AddAdjacentNode(0, _grain, _grain, parent); //down
AddAdjacentNode(-_grain, 0, _grain, parent); //left
}
else if (parent.Direction == NodeDirection.Up)
{
AddAdjacentNode(0, _grain, _grain, parent); //down
AddAdjacentNode(_grain, 0, _grain, parent); //right
AddAdjacentNode(-_grain, 0, _grain, parent); //left
AddAdjacentNode(0, -_grain, _grain, parent); //up
}
}
return(false);
}
