/// <summary> /// Prints the solution /// </summary> /// <param name="ASolution">The list that holds the solution</param> public static void PrintSolution(ArrayList ASolution) { for (int j = 0; j < 10; j++) { for (int i = 0; i < 10; i++) { bool solution = false; foreach (AStarNode2D n in ASolution) { AStarNode2D tmp = new AStarNode2D(null, null, 0, i, j); solution = n.IsSameState(tmp); if (solution) { break; } } if (solution) { Console.Write("S "); } else if (GetMap(i, j) == -1) { Console.Write("X "); } else { Console.Write(". "); } } Console.WriteLine(""); } }
/// <summary> /// Adds a successor to a list if it is not impassible or the parent node /// </summary> /// <param name="ASuccessors">List of successors</param> /// <param name="AX">X-coordinate</param> /// <param name="AY">Y-coordinate</param> void AddSuccessor(ArrayList ASuccessors, int AX, int AY) { int CurrentCost = StartPath.GetMap(AX, AY); if (CurrentCost == -1) { return; } AStarNode2D NewNode = new AStarNode2D(this, GoalNode, Cost + CurrentCost, AX, AY); if (NewNode.IsSameState(Parent)) { return; } ASuccessors.Add(NewNode); }
static void Main(string[] args) { Console.WriteLine("Starting..."); AStar astar = new AStar(); AStarNode2D GoalNode = new AStarNode2D(null, null, 0, 9, 9); AStarNode2D StartNode = new AStarNode2D(null, GoalNode, 0, 0, 0) { GoalNode = GoalNode }; astar.FindPath(StartNode, GoalNode); PrintSolution(astar.Solution); Console.ReadLine(); }
public static List <string> Path(int startx, int starty, int endx, int endy, int endz, int csx, int csy) { // Here is where we come in from BotMe with our start and end points from the world AStar astar = new AStar(); AStarNode2D GoalNode = new AStarNode2D(null, null, 0, endx, endy); AStarNode2D StartNode = new AStarNode2D(null, GoalNode, 0, startx, starty) { GoalNode = GoalNode }; // Prepare the final List that will become the waypoints for him to leaf through List <string> botPoint = new List <string>(); // Go get the solution astar.FindPath(StartNode, GoalNode); // First check if the path was possible bool pathDone = astar.pathPossible; if (pathDone == false) { //Use botPoint List as a flag to break out of this. Return to Botme botPoint.Add("no_path"); return(botPoint); } // Slope calculation data int slope = 99; // Use 99 here to mean the slope has never been calculated yet int lastSlope = 99; int X1 = startx; //startx int Y1 = starty; //starty int Z = endz; //startz - we need this to make a vector but will override with current z in Botme enabling him to walk up hills int xtemp = 0; int ytemp = 0; // This gets the solution from Astar.cs and runs it through PrintInfo which has the xyz of each path node - our Node solution ArrayList Nodes = new ArrayList(astar.Solution); foreach (AStarNode nn in Nodes) { AStarNode2D n = (AStarNode2D)nn; // Return x and y from printinfo int[] XYreturn = new int[2]; XYreturn = n.PrintNodeInfo(); int X2 = XYreturn[0]; int Y2 = XYreturn[1]; // Here I calculate point only where the line changes direction // In this way the bot doesn't start and stop each step // Since it has been determined that the path is clear between these points this will work // You can see the trouble with moving objects here though - he will have to constantly check on the way to these points // To detect scene changes. slope = calcSlope(Y2, Y1, X2, X1); if (lastSlope != slope) { // Build the list of waypoints only where changes of slope occur xtemp = X1 + csx; //conerStone x and y from our map to get these into sim coordinates ytemp = Y1 + csy; string temp = xtemp.ToString() + "," + ytemp.ToString() + "," + Z.ToString(); botPoint.Add(temp); } X1 = X2; Y1 = Y2; lastSlope = slope; } // This adds the last point to the step xtemp = X1 + csx; ytemp = Y1 + csy; string temp2 = xtemp.ToString() + "," + ytemp.ToString() + "," + Z.ToString(); botPoint.Add(temp2); // This removes the first point of the steps so they turn and go right to the first bend point(slope) botPoint.RemoveRange(0, 1); // Let em have it - return to Botme path with slopes only no start point but with end point always return(botPoint); }