private Point getNextPoint(Point sourceLoc, Point targetLoc) { // clear the lists openList.Clear(); closedList.Clear(); // find direction of targetLoc GridFacing facing = findFacing(sourceLoc, targetLoc); // get starting adjacent locations foreach (Node node in getNeighbors(new Node(sourceLoc.X, sourceLoc.Y), (int)facing, targetLoc)) { openList.Add(node); } Node previous = new Node(); while (openList.Count > 0) { // current lowest G+H int indexS = openList.Min(f => f.F); // get next node with lowest G+H that is not the previous node Node node = openList.First(f => f.F == indexS && f != previous); // check for goal if (node.Loc == targetLoc) { return(recurseNextNode(node).Loc); } // remove from openset openList.Remove(node); // add to closedset closedList.Add(node); // get neighbors of current location List <Node> sortedNeighbors = getNeighbors(node, (int)facing, targetLoc); foreach (Node neighbor in sortedNeighbors) { // not in closed set if (closedList.Contains(neighbor)) { continue; } // steps from source int tempG = node.G + 1; // not already in openset or temp g less than existing g; if (!openList.Any(f => f.Loc == neighbor.Loc && f.G <= tempG))//|| tempG < neighbor.G) { neighbor.Parent = node; neighbor.G = tempG; neighbor.H = Convert.ToInt32(neighbor.Loc.DistanceTo(targetLoc)); if (!openList.Contains(neighbor) && !closedList.Contains(neighbor)) { openList.Add(neighbor); } } } previous = node; } return(new Point(0, 0)); }
private Node findNeighborInDirection(Node source, GridFacing direction) { Node result = new Node(); switch (direction) { case GridFacing.NorthEast: result = new Node(source.Loc.X + 1, source.Loc.Y - 1); break; case GridFacing.East: result = new Node(source.Loc.X + 1, source.Loc.Y); break; case GridFacing.SouthEast: result = new Node(source.Loc.X + 1, source.Loc.Y + 1); break; case GridFacing.North: result = new Node(source.Loc.X, source.Loc.Y - 1); break; case GridFacing.South: result = new Node(source.Loc.X, source.Loc.Y + 1); break; case GridFacing.NorthWest: result = new Node(source.Loc.X - 1, source.Loc.Y - 1); break; case GridFacing.West: result = new Node(source.Loc.X - 1, source.Loc.Y); break; case GridFacing.SouthWest: result = new Node(source.Loc.X - 1, source.Loc.Y + 1); break; } return(result); }