public bool isMatch(Node n)
{
if (n!=null)
return (x==n.x && y==n.y);
else
return false;
}
public Node(Node parentNode, Node goalNode, int gCost,int x, int y)
{
this.parentNode = parentNode;
this._goalNode = goalNode;
this.gCost = gCost;
this.x=x;
this.y=y;
InitNode();
}
public int IndexOf(Node n)
{
for (int i =0; i< _list.Count ;i++)
{
Node nodeInTheList = (Node) _list[i];
if (nodeInTheList.isMatch (n))
return i;
}
return -1;
}
public int push(Node n)
{
int k = _list.BinarySearch (n,_nodeComparer);
if (k==-1) // no element
_list.Insert (0,n);
else if (k<0) // find location by complement
{
k=~k;
_list.Insert (k,n);
}
else if (k>=0)
_list.Insert (k,n);
return k;
}
public ArrayList GetSuccessors()
{
ArrayList successors = new ArrayList ();
for (int xd=-1;xd<=1;xd++)
{
for (int yd=-1;yd<=1;yd++)
{
if (Math.Abs(xd) + Math.Abs(yd) == 2) continue;
else if (Map.getMap (x+xd,y+yd) !=-1)
{
Node n = new Node (this,this._goalNode ,Map.getMap (x+xd,y+yd) ,x+xd,y+yd);
if (!n.isMatch (this.parentNode) && !n.isMatch (this))
successors.Add (n);
}
}
}
return successors;
}
public static string[,] PrintSolution(ArrayList solutionPathList)
{
int yMax =Mapdata.GetUpperBound (0);
int xMax =Mapdata.GetUpperBound (1);
string[,] ans = new string[size, size];
for(int j=0;j<=yMax;j++)
{
for(int i=0;i<=xMax;i++)
{
bool solutionNode = false;
foreach(Node n in solutionPathList)
{
Node tmp = new Node(null,null,0,i,j);
if(n.isMatch (tmp))
{
solutionNode = true;
break;
}
}
if(solutionNode){
Console.Write("o "); //solution path
ans[j,i]="o";
}
else if (Map.getMap(i, j) == -1)
{
Console.Write("# "); //wall
ans[j,i] = "#";
}
else
{
Console.Write(". "); //road
ans[j,i] = ".";
}
}
Console.WriteLine("");
}
return ans;
}
public static string[,] find_shortest_path(int[,] new_array,int sx,int sy,int ex,int ey)
{
Map m = new Map(new_array);
ArrayList SolutionPathList = new ArrayList();
//Create a node containing the goal state node_goal
Node node_goal = new Node(null, null, 1, ex, ey);
//Create a node containing the start state node_start
Node node_start = new Node(null, node_goal, 1, sx, sy);
//Create OPEN and CLOSED list
SortedCostNodeList OPEN = new SortedCostNodeList();
SortedCostNodeList CLOSED = new SortedCostNodeList();
//Put node_start on the OPEN list
OPEN.push(node_start);
//while the OPEN list is not empty
while (OPEN.Count > 0)
{
//Get the node off the open list
//with the lowest f and call it node_current
Node node_current = OPEN.pop();
//if node_current is the same state as node_goal we have found the solution;
//break from the while loop;
if (node_current.isMatch(node_goal))
{
node_goal.parentNode = node_current.parentNode;
break;
}
//Generate each state node_successor that can come after node_current
ArrayList successors = node_current.GetSuccessors();
//for each node_successor or node_current
foreach (Node node_successor in successors)
{
//Set the cost of node_successor to be the cost of node_current plus
//the cost to get to node_successor from node_current
//--> already set while we are getting successors
//find node_successor on the OPEN list
int oFound = OPEN.IndexOf(node_successor);
//if node_successor is on the OPEN list but the existing one is as good
//or better then discard this successor and continue
if (oFound > 0)
{
Node existing_node = OPEN.NodeAt(oFound);
if (existing_node.CompareTo(node_current) <= 0)
continue;
}
//find node_successor on the CLOSED list
int cFound = CLOSED.IndexOf(node_successor);
//if node_successor is on the CLOSED list but the existing one is as good
//or better then discard this successor and continue;
if (cFound > 0)
{
Node existing_node = CLOSED.NodeAt(cFound);
if (existing_node.CompareTo(node_current) <= 0)
continue;
}
//Remove occurences of node_successor from OPEN and CLOSED
if (oFound != -1)
OPEN.RemoveAt(oFound);
if (cFound != -1)
CLOSED.RemoveAt(cFound);
//Set the parent of node_successor to node_current;
//--> already set while we are getting successors
//Set h to be the estimated distance to node_goal (Using heuristic function)
//--> already set while we are getting successors
//Add node_successor to the OPEN list
OPEN.push(node_successor);
}
//Add node_current to the CLOSED list
CLOSED.push(node_current);
}
//follow the parentNode from goal to start node
//to find solution
Node p = node_goal;
while (p != null)
{
SolutionPathList.Insert(0, p);
p = p.parentNode;
}
//display solution
string[,] ans=Map.PrintSolution(SolutionPathList);
return ans;
}
