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