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