// an A* shortest path algorithm
public APath shortestPath(int startIpos, int startJpos, int destIpos, int destJpos,
MapCell[] Visited, int VisitedLength, ref Boolean PathFound)
{
int i;
int newVisitedLength = VisitedLength + 1;
MapCell[] newVisited = new MapCell[newVisitedLength];
APath shortestpath = null;
// first checking whether we are in /out of map bounds
if ((startIpos < 0) || (startIpos >= mappingFSM.MapDim) || (startJpos < 0) || (startJpos >= mappingFSM.MapDim))
PathFound = false;
else
if (mappingFSM.Map[startIpos][startJpos] == MappingFSM.cellBLOCK)
PathFound = false;
else
if (inPath(startIpos, startJpos, Visited, VisitedLength))
PathFound = false;
else if ((mappingFSM.ipos == startIpos) && (mappingFSM.jpos == startJpos))
PathFound = false;
else
{
// copying map of visited cells
for (i = 0; i < VisitedLength; i++)
newVisited[i] = Visited[i];
// visited cells list copied
// now adding current position
newVisited[newVisitedLength - 1].ipos = startIpos;
newVisited[newVisitedLength - 1].jpos = startJpos;
// checking northern cell
if ((startIpos - 1 == destIpos) && (startJpos == destJpos))
{
PathFound = true;
shortestpath = new APath(1);
shortestpath.amoves[0] = APath.instGO_NORTH;
} // checking eastern cell
else if ((startIpos == destIpos) && (startJpos + 1 == destJpos))
{
PathFound = true;
shortestpath = new APath(1);
shortestpath.amoves[0] = APath.instGO_EAST;
} // checking southern cell
else if ((startIpos + 1 == destIpos) && (startJpos == destJpos))
{
PathFound = true;
shortestpath = new APath(1);
shortestpath.amoves[0] = APath.instGO_SOUTH;
}// checking western cell
else if ((startIpos == destIpos) && (startJpos - 1 == destJpos))
{
PathFound = true;
shortestpath = new APath(1);
shortestpath.amoves[0] = APath.instGO_WEST;
}// recursively checking paths now...
else
{
// sorting out euclidean distances of neighboring cells
double[] distances = new double[4]; // 0-north, 1-east, 2-south, 3-west
for (i = 0; i < 4; i++)
distances[i] = Math.Sqrt(Math.Pow((double)(startIpos - destIpos), 2) + Math.Pow((double)(startJpos - destJpos), 2));
// now repeating until we find a valid path
Boolean pfound = false;
PathFound = false;
int counter = 0;
double mindistance = 100000;
int mindistanceindex = 0;
APath newpath;
while ((!pfound) && (counter < 4))
{
mindistance = 100000; // large enough value for minimum euclidean distance
for (i = 0; i < 4; i++)
if ((distances[i] < mindistance) && (distances[i] >= 0))
{
mindistance = distances[i];
mindistanceindex = i;
}
if (mindistance < 100000)
{
distances[mindistanceindex] = -1;
switch (mindistanceindex)
{
case 0: // North
newpath = shortestPath(startIpos - 1, startJpos, destIpos, destJpos,
newVisited, newVisitedLength, ref pfound);
if (pfound)
{
PathFound = true;
shortestpath = new APath(1);
shortestpath.amoves[0] = APath.instGO_NORTH;
shortestpath.appendPath(newpath);
}
break;
case 1: // East
newpath = shortestPath(startIpos, startJpos + 1, destIpos, destJpos,
newVisited, newVisitedLength, ref pfound);
if (pfound)
{
PathFound = true;
shortestpath = new APath(1);
shortestpath.amoves[0] = APath.instGO_EAST;
shortestpath.appendPath(newpath);
}
break;
case 2: // South
newpath = shortestPath(startIpos + 1, startJpos, destIpos, destJpos,
newVisited, newVisitedLength, ref pfound);
if (pfound)
{
PathFound = true;
shortestpath = new APath(1);
shortestpath.amoves[0] = APath.instGO_SOUTH;
shortestpath.appendPath(newpath);
}
break;
case 3: // South
newpath = shortestPath(startIpos, startJpos - 1, destIpos, destJpos,
newVisited, newVisitedLength, ref pfound);
if (pfound)
{
PathFound = true;
shortestpath = new APath(1);
shortestpath.amoves[0] = APath.instGO_WEST;
shortestpath.appendPath(newpath);
}
break;
}
}
counter++;
}
} // recursion else ends
} // big else ends
return shortestpath;
}
public Boolean inPath(int ipos, int jpos, MapCell[] path, int pathlen)
{
int i;
Boolean found = false;
i = 0;
while ((i < pathlen) && (!found))
{
found = (path[i].ipos == ipos) && (path[i].jpos == jpos) ? true : found;
i++;
}
return found;
}