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)
{
//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 + "," + ytemp + "," + Z;
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 + "," + ytemp + "," + Z;
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;
}
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();
}