//Methods
public void Reset()
{
mOpenList = new BinaryHeap();
mGrid = new BinaryGrid();
mGeneration = 1;
mSolution = null;
mGoal = null;
}
//Methods
public void Reset()
{
_movementList.Clear();
_openList.Clear();
_grid = new BinaryGrid <RouteNode>();
_nodeFactory.Clear();
_solution = null;
_goal = null;
}
//Add a node to a parent node, ignoring if impassable or already closed,
//adding if new, updating if new and better route
private RouteNode AddAdjacentNode(int dx, int dy, int cost, RouteNode parent)
{
int x = parent.X + dx;
int y = parent.Y + dy;
int newCost = parent.MovementCost + cost;
//Get terrain cost. -1 is not passable, 0 no cost, 1,2,3 etc higher cost
int terraincost = GetTerrainCost(x, y);
if (terraincost == -1)
{
return(null);
}
//Check if item has been added to the grid already
RouteNode existing = (RouteNode)mGrid.Item(x, y);
if (existing != null)
{
if (!existing.Closed && newCost < existing.MovementCost)
{
existing.MovementCost = newCost;
existing.TotalCost = existing.MovementCost + existing.Heuristic;
existing.Parent = parent;
mOpenList.Update(existing);
}
return(existing);
}
//Create a new node
RouteNode node = new RouteNode();
node.Parent = parent;
node.X = x;
node.Y = y;
node.MovementCost = newCost; //Add the cost to the parent cost
node.Heuristic = GetHeuristic(node);
node.TotalCost = node.MovementCost + node.Heuristic;
//Add terrain cost
node.MovementCost += terraincost;
node.TotalCost += terraincost;
//Set generation
node.Generation = mGeneration;
mGeneration++;
//Add to the open list and the grid
mOpenList.Push(node);
mGrid.Add(node);
return(node);
}
//Add a node to a parent node, ignoring if impassable or already closed,
//adding if new, updating if new and better route
private void AddAdjacentNode(int dx, int dy, int cost, RouteNode parent)
{
int x = parent.X + dx;
int y = parent.Y + dy;
int newCost = parent.MovementCost + cost;
//Get terrain cost. -1 is not passable, 0 no cost, 1,2,3 etc higher cost
int terraincost = GetTerrainCost(x, y);
if (terraincost == -1)
{
return;
}
//Check if item has been added to the grid already
RouteNode existing = _grid.Get(x, y);
if (existing != null)
{
if (!existing.Closed && newCost < existing.MovementCost)
{
existing.MovementCost = newCost;
existing.TotalCost = newCost + existing.Heuristic;
existing.Parent = parent;
if (_movementList.Contains(existing))
{
_movementList.Update(existing);
}
else
{
_openList.Update(existing);
}
}
return;
}
//Create a new node
RouteNode node = new RouteNode();
node.X = x;
node.Y = y;
//Set the parent after the location so that the Direction can be set correctly
node.Parent = parent;
node.MovementCost = newCost; //Add the cost to the parent cost
node.Heuristic = GetHeuristic(node);
node.TotalCost = newCost + node.Heuristic;
PushNode(node);
//Add to the grid
_grid.Add(x, y, node);
}
public List <PointF> GetRoute(PointF begin, PointF end)
{
//Move the begin and end onto the grid
Point beginAligned = Point.Round(begin);
Point endAligned = Point.Round(end);
beginAligned = new Point((beginAligned.X / _grain) * _grain, (beginAligned.Y / _grain) * _grain);
endAligned = new Point((endAligned.X / _grain) * _grain, (endAligned.Y / _grain) * _grain);
//Recreate the rectangles surrounding the shapes in the model
if (_terrain.Count == 0)
{
CreateTerrain(beginAligned, endAligned);
}
//Check for exclusions
if (Start != null && End != null)
{
Rectangle startRect = RectangleToGrid(GetInflatedRectangle(Start));
Rectangle endRect = RectangleToGrid(GetInflatedRectangle(End));
startRect.Inflate(_grain, _grain);
endRect.Inflate(_grain, _grain);
//If rectangles intersect or are adjacent then just make connector
if (startRect.IntersectsWith(endRect) || Geometry.AreAdjacent(startRect, endRect))
{
return(MakeConnector(begin, end));
}
}
//Check to see if a route must be calculated, or just a connector shape created
if (_terrain != null && !_terrain.IsEmpty)
{
RouteNode start = _nodeFactory.Create();
RouteNode goal = _nodeFactory.Create();
start.SetPoint(beginAligned);
goal.SetPoint(endAligned);
if (CalculateNodes(start, goal))
{
List <PointF> solution = GetSolution();
if (solution != null && solution.Count > 1)
{
AlignSolution(solution, begin, end);
return(solution);
}
}
}
return(MakeConnector(begin, end));
}
//Pops the most recent lowest f cost node off the heap
private RouteNode GetNextNode()
{
RouteNode pop = (RouteNode)mOpenList.Pop();
if (mOpenList.Count < 2)
{
return(pop);
}
RouteNode peek = (RouteNode)mOpenList.Peek();
return(pop);
}
//Calculates the distance as the best guess distance to move to goal.
//Must not be greater than possible distance
//More accurately the heurisitc must be the cost of getting to the current node + the shortest distance estimate to complete
//However we take the movement cost into account later using the routenode's total cost
//We can use the shortest straight line distance to the goal as the second part of the heuristic.
//or simply calculate the orthoganal distance
private int GetHeuristic(RouteNode current)
{
//Euclidean
//double x2 = Math.Pow(Math.Abs(_goal.X - current.X), 2);
//double y2 = Math.Pow(Math.Abs(_goal.Y - current.Y), 2);
//return Convert.ToInt32(Math.Sqrt(x2 + y2));
//Manhattan
int x = Math.Abs(_goal.X - current.X);
int y = Math.Abs(_goal.Y - current.Y);
return(x + y);
}
public ArrayList GetRoute(PointF begin, PointF end)
{
//Move the begin and end onto the grid
Point beginAligned = Point.Round(begin);
Point endAligned = Point.Round(end);
beginAligned = new Point((beginAligned.X / mGrain) * mGrain, (beginAligned.Y / mGrain) * mGrain);
endAligned = new Point((endAligned.X / mGrain) * mGrain, (endAligned.Y / mGrain) * mGrain);
//Recreate the rectangles surrounding the shapes in the container
if (mTerrain == null)
{
CreateTerrain(beginAligned, endAligned);
}
RouteNode start = new RouteNode(beginAligned.X, beginAligned.Y);
RouteNode goal = new RouteNode(endAligned.X, endAligned.Y);
//Check for exclusions
if (Start != null && End != null)
{
Rectangle startRect = RectangleToGrid(GetInflatedRectangle(Start));
Rectangle endRect = RectangleToGrid(GetInflatedRectangle(End));
startRect.Inflate(mGrain, mGrain);
endRect.Inflate(mGrain, mGrain);
//If rectangles intersect or are adjacent then just make connector
if (startRect.IntersectsWith(endRect) || Geometry.AreAdjacent(startRect, endRect))
{
return(MakeConnector(begin, end));
}
}
//Check to see if a route must be calculated, or just a connector shape created
if (mTerrain != null && !mTerrain.IsEmpty)
{
if (CalculateNodes(start, goal))
{
ArrayList solution = GetSolution();
if (solution != null && solution.Count > 1)
{
AlignSolution(solution, begin, end);
return(solution);
}
}
}
return(MakeConnector(begin, end));
}
//Returns the next node in the open list
private RouteNode PopNode()
{
//Move more nodes onto the movement list if there are none
if (_movementList.Count == 0)
{
RouteNode node = _openList.Pop();
_movementList.Push(node);
//Keep moving nodes with the same totalcost onto the movement list
//So that they can be sorted by movement cost
while (_openList.Count > 0 && node.TotalCost == _openList.TotalCost)
{
_movementList.Push(_openList.Pop());
}
}
return(_movementList.Pop());
}
//Pushes the node supplied onto the correct list
private void PushNode(RouteNode node)
{
//Add to the movement or open list
//Add to movement if both counts zero
if (_movementList.Count == 0 && _openList.Count == 0)
{
_movementList.Push(node);
}
//Decide when movement list count > zero
else if (_movementList.Count > 0)
{
if (node.TotalCost <= _movementList.TotalCost)
{
//Move all nodes from the movement list to the open list that are greater than the totalcost of the new node
if (node.TotalCost < _movementList.TotalCost)
{
while (_movementList.Peek() != null)
{
_openList.Push(_movementList.Pop());
}
}
_movementList.Push(node);
}
else
{
_openList.Push(node);
}
}
//openlist count > 0
else
{
if (node.TotalCost < _openList.TotalCost)
{
_movementList.Push(node);
}
else
{
_openList.Push(node);
}
}
}
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);
}
//Calculates the manhattan distance as the best guess distance to move to goal.
//Must not be greater than possible distance
//We use a unit of 10 for each 1 unit of movement
private int GetHeuristic(RouteNode current)
{
return mModifier * (Math.Abs(mGoal.X - current.X) + Math.Abs(mGoal.Y - current.Y));
}
//Add a node to a parent node, ignoring if impassable or already closed,
//adding if new, updating if new and better route
private RouteNode AddAdjacentNode(int dx, int dy, int cost, RouteNode parent)
{
int x = parent.X + dx;
int y = parent.Y + dy;
int newCost = parent.MovementCost + cost;
//Get terrain cost. -1 is not passable, 0 no cost, 1,2,3 etc higher cost
int terraincost = GetTerrainCost(x, y);
if (terraincost == -1) return null;
//Check if item has been added to the grid already
RouteNode existing = (RouteNode) mGrid.Item(x, y);
if (existing != null)
{
if (!existing.Closed && newCost < existing.MovementCost)
{
existing.MovementCost = newCost;
existing.TotalCost = existing.MovementCost + existing.Heuristic;
existing.Parent = parent;
mOpenList.Update(existing);
}
return existing;
}
//Create a new node
RouteNode node = new RouteNode();
node.Parent = parent;
node.X = x;
node.Y = y;
node.MovementCost = newCost; //Add the cost to the parent cost
node.Heuristic = GetHeuristic(node);
node.TotalCost = node.MovementCost + node.Heuristic;
//Add terrain cost
node.MovementCost += terraincost;
node.TotalCost += terraincost;
//Set generation
node.Generation = mGeneration;
mGeneration ++;
//Add to the open list and the grid
mOpenList.Push(node);
mGrid.Add(node);
return node;
}
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;
}
public ArrayList GetRoute(PointF begin, PointF end)
{
//Move the begin and end onto the grid
Point beginAligned = Point.Round(begin);
Point endAligned = Point.Round(end);
beginAligned = new Point((beginAligned.X / mGrain) * mGrain, (beginAligned.Y / mGrain) * mGrain);
endAligned = new Point((endAligned.X / mGrain) * mGrain, (endAligned.Y / mGrain) * mGrain);
//Recreate the rectangles surrounding the shapes in the container
if (mTerrain == null) CreateTerrain(beginAligned, endAligned);
RouteNode start = new RouteNode(beginAligned.X, beginAligned.Y);
RouteNode goal = new RouteNode(endAligned.X, endAligned.Y);
//Check for exclusions
if (Start != null && End != null)
{
Rectangle startRect = RectangleToGrid(GetInflatedRectangle(Start));
Rectangle endRect = RectangleToGrid(GetInflatedRectangle(End));
startRect.Inflate(mGrain, mGrain);
endRect.Inflate(mGrain, mGrain);
//If rectangles intersect or are adjacent then just make connector
if (startRect.IntersectsWith(endRect) || Geometry.AreAdjacent(startRect, endRect))
{
return MakeConnector(begin, end);
}
}
//Check to see if a route must be calculated, or just a connector shape created
if (mTerrain != null && !mTerrain.IsEmpty)
{
if (CalculateNodes(start, goal))
{
ArrayList solution = GetSolution();
if (solution != null && solution.Count > 1)
{
AlignSolution(solution, begin, end);
return solution;
}
}
}
return MakeConnector(begin, end);
}
//Methods
public void Reset()
{
mOpenList = new BinaryHeap();
mGrid = new BinaryGrid();
mGeneration = 1;
mSolution = null;
mGoal = null;
}
public bool Near(RouteNode node, int grain)
{
return(Math.Abs(node.X - X) < grain && Math.Abs(node.Y - Y) < grain);
}
public bool Equals(RouteNode node)
{
return(node.X == X && node.Y == Y);
}
public bool Equals(RouteNode node)
{
return (node.X == X && node.Y == Y);
}
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);
}
//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);
}
public bool Near(RouteNode node, int grain)
{
return (Math.Abs(node.X - X) < grain && Math.Abs(node.Y - Y) < grain);
}
//Calculates the manhattan distance as the best guess distance to move to goal.
//Must not be greater than possible distance
//We use a unit of 10 for each 1 unit of movement
private int GetHeuristic(RouteNode current)
{
return(mModifier * (Math.Abs(mGoal.X - current.X) + Math.Abs(mGoal.Y - current.Y)));
}
