public node() {
this.selfNodexy = new xypoint();
this.costFromStart = 0;
this.costToGoal = 0;
this.totalCost = this.costFromStart + this.costToGoal;
this.parentNodexy = new xypoint();
this.terrain = 0;
}
static void Main(string[] args)
{
xypoint startNode = new xypoint(1,2);
xypoint goalNode = new xypoint(3,5);
String words = helperfunction.AstarSearch(startNode, goalNode);
Console.WriteLine(words);
}
static public String AstarSearch(xypoint startNode, xypoint goalNode) {
String returnString = "No routes to reach the goal";
//TODO need to be fixed
node[,] map = buildMap(8, 8);
modifyMap(map);
List<node> open = new List<node>();
List<node> closed = new List<node>();
// this is the initial node
map[startNode.x, startNode.y].costToGoal = PathCostEstimation(startNode, goalNode);
map[startNode.x, startNode.y].totalCost = map[startNode.x, startNode.y].costToGoal;
open.Add(map[startNode.x, startNode.y]);
while (open.Count>0) {
int index = lowestTotalCost(open);
node Node = pop(open, index);
if (Node.selfNodexy.x == goalNode.x && Node.selfNodexy.y == goalNode.y)
{
Console.WriteLine("Goal("+goalNode.x+","+goalNode.y+")");
xypoint print = Node.parentNodexy;
while (print.x != 0 && print.y != 0) {
Console.WriteLine("("+print.x+","+ print.y+")");
print = map[print.x, print.y].parentNodexy;
}
returnString = "success";
break;
}
else {
List<node> SuccessorNodes = successorNodes(Node, map);
foreach (node NewNode in SuccessorNodes) {
float NewCost = Node.costFromStart + TraverseCost(NewNode.selfNodexy, Node.selfNodexy);
if (((open.IndexOf(NewNode) != -1) || (closed.IndexOf(NewNode) != -1)) &&
NewNode.costFromStart <= NewCost)
{
continue;
}
else {
map[NewNode.selfNodexy.x, NewNode.selfNodexy.y].parentNodexy = Node.selfNodexy;
map[NewNode.selfNodexy.x, NewNode.selfNodexy.y].costFromStart = NewCost;
map[NewNode.selfNodexy.x, NewNode.selfNodexy.y].costToGoal = PathCostEstimation(NewNode.selfNodexy, goalNode);
map[NewNode.selfNodexy.x, NewNode.selfNodexy.y].totalCost =
map[NewNode.selfNodexy.x, NewNode.selfNodexy.y].costFromStart +
map[NewNode.selfNodexy.x, NewNode.selfNodexy.y].costToGoal;
if (closed.IndexOf(NewNode) != -1) {
closed.Remove(NewNode);
}
if (open.IndexOf(NewNode) != -1)
{
open.Remove(NewNode);
open.Add(map[NewNode.selfNodexy.x, NewNode.selfNodexy.y]);
}
else {
open.Add(NewNode);
}
}
}
}
closed.Add(Node);
}
return returnString;
}
static private float PathCostEstimation(xypoint startNode, xypoint goalNode) {
return (float)Math.Sqrt(Math.Pow(startNode.x - goalNode.x, 2) + Math.Pow(startNode.y - goalNode.y, 2));
}
static private float TraverseCost(xypoint startNode, xypoint endNode) {
return PathCostEstimation(startNode, endNode);
}