public void appendPath(APath path)
{
int[] tempmoves = new int[pathlen + path.pathlen];
int i;
for (i = 0; i < pathlen+path.pathlen; i++)
tempmoves[i] = (i < pathlen) ? amoves[i] : path.amoves[i - pathlen];
amoves = tempmoves;
pathlen += path.pathlen;
}
public void appendPath(APath path)
{
int[] tempmoves = new int[pathlen + path.pathlen];
int i;
for (i = 0; i < pathlen + path.pathlen; i++)
{
tempmoves[i] = (i < pathlen) ? amoves[i] : path.amoves[i - pathlen];
}
amoves = tempmoves;
pathlen += path.pathlen;
}
// fill the task queue with a number of moves
// to follow a certain path
public void path2QueueTasks(APath path, int corientation)
{
int i;
int curOrient = corientation;
for (i = 0; i < path.pathlen; i++)
{
switch (curOrient)
{
case orNORTH:
switch (path.amoves[i])
{
case APath.instGO_NORTH:
insertTask(taskGO_FORWARD);
break;
case APath.instGO_EAST:
insertTask(taskTURN_RIGHT);
insertTask(taskGO_FORWARD);
curOrient = orEAST;
break;
case APath.instGO_SOUTH:
insertTask(taskGO_BACKWARD);
break;
case APath.instGO_WEST:
insertTask(taskTURN_LEFT);
insertTask(taskGO_FORWARD);
curOrient = orWEST;
break;
}
break;
case orEAST:
switch (path.amoves[i])
{
case APath.instGO_NORTH:
insertTask(taskTURN_LEFT);
insertTask(taskGO_FORWARD);
curOrient = orNORTH;
break;
case APath.instGO_EAST:
insertTask(taskGO_FORWARD);
break;
case APath.instGO_SOUTH:
insertTask(taskTURN_RIGHT);
insertTask(taskGO_FORWARD);
curOrient = orSOUTH;
break;
case APath.instGO_WEST:
insertTask(taskGO_BACKWARD);
break;
}
break;
case orSOUTH:
switch (path.amoves[i])
{
case APath.instGO_NORTH:
insertTask(taskGO_BACKWARD);
break;
case APath.instGO_EAST:
insertTask(taskTURN_LEFT);
insertTask(taskGO_FORWARD);
curOrient = orEAST;
break;
case APath.instGO_SOUTH:
insertTask(taskGO_FORWARD);
break;
case APath.instGO_WEST:
insertTask(taskTURN_RIGHT);
insertTask(taskGO_FORWARD);
curOrient = orWEST;
break;
}
break;
case orWEST:
switch (path.amoves[i])
{
case APath.instGO_NORTH:
insertTask(taskTURN_RIGHT);
insertTask(taskGO_FORWARD);
curOrient = orNORTH;
break;
case APath.instGO_EAST:
insertTask(taskGO_BACKWARD);
break;
case APath.instGO_SOUTH:
insertTask(taskTURN_LEFT);
insertTask(taskGO_FORWARD);
curOrient = orSOUTH;
break;
case APath.instGO_WEST:
insertTask(taskGO_FORWARD);
break;
}
break;
}
}
insertTask(taskHUG);
}
public void planner()
{
int i, j;
if ((mappingFSM.ipos != LastKnownCartIpos) || (mappingFSM.jpos != LastKnownCartJpos))
{
// the mobile robot has changed position,
// thus we may need to follow a different path to get to it
// updating last known cart position
LastKnownCartIpos = mappingFSM.ipos;
LastKnownCartJpos = mappingFSM.jpos;
// finding the closest reachable cell next to the cart
int minpathlen = 1000000, mini, minj;
APath minpath = null;
for (i = 0; i < mappingFSM.MapDim; i++)
{
for (j = 0; j < mappingFSM.MapDim; j++)
{
if (mappingFSM.Map[i][j] == cellVOID)
{
if (isNeighborCart(i, j))
{
Boolean pfound = false;
APath path2cell = shortestPath(ipos, jpos, i, j, null, 0, ref pfound);
if (pfound)
{
if (minpathlen > path2cell.pathlen)
{
minpathlen = path2cell.pathlen;
minpath = path2cell;
mini = i;
minj = j;
}
}
}
}
}
}
// clearing the task queue anyway
TaskQueueLength = 0;
if (minpath == null) // no path close to the cell found
// adding a reaction
{
insertTask(taskDISSAPOINTED);
}
else // planning to move to the cell
{
path2QueueTasks(minpath, orientation);
}
}
else
if ((!close2Cart()) && (TaskQueueLength == 0))
{
// finding the closest reachable cell next to the cart
int minpathlen = 1000000, mini, minj;
APath minpath = null;
for (i = 0; i < mappingFSM.MapDim; i++)
{
for (j = 0; j < mappingFSM.MapDim; j++)
{
if (mappingFSM.Map[i][j] == cellVOID)
{
if (isNeighborCart(i, j))
{
Boolean pfound = false;
APath path2cell = shortestPath(ipos, jpos, i, j, null, 0, ref pfound);
if (pfound)
{
if (minpathlen > path2cell.pathlen)
{
minpathlen = path2cell.pathlen;
minpath = path2cell;
mini = i;
minj = j;
}
}
}
}
}
}
// clearing the task queue anyway
TaskQueueLength = 0;
if (minpath == null) // no path close to the cell found
// adding a reaction
{
insertTask(taskDISSAPOINTED);
}
else // planning to move to the cell
{
path2QueueTasks(minpath, orientation);
}
}
}
// 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);
}
// 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;
}
// fill the task queue with a number of moves
// to follow a certain path
public void path2QueueTasks(APath path, int corientation)
{
int i;
int curOrient = corientation;
for (i = 0; i < path.pathlen; i++)
switch (curOrient)
{
case orNORTH:
switch (path.amoves[i])
{
case APath.instGO_NORTH:
insertTask(taskGO_FORWARD);
break;
case APath.instGO_EAST:
insertTask(taskTURN_RIGHT);
insertTask(taskGO_FORWARD);
curOrient = orEAST;
break;
case APath.instGO_SOUTH:
insertTask(taskGO_BACKWARD);
break;
case APath.instGO_WEST:
insertTask(taskTURN_LEFT);
insertTask(taskGO_FORWARD);
curOrient = orWEST;
break;
}
break;
case orEAST:
switch (path.amoves[i])
{
case APath.instGO_NORTH:
insertTask(taskTURN_LEFT);
insertTask(taskGO_FORWARD);
curOrient = orNORTH;
break;
case APath.instGO_EAST:
insertTask(taskGO_FORWARD);
break;
case APath.instGO_SOUTH:
insertTask(taskTURN_RIGHT);
insertTask(taskGO_FORWARD);
curOrient = orSOUTH;
break;
case APath.instGO_WEST:
insertTask(taskGO_BACKWARD);
break;
}
break;
case orSOUTH:
switch (path.amoves[i])
{
case APath.instGO_NORTH:
insertTask(taskGO_BACKWARD);
break;
case APath.instGO_EAST:
insertTask(taskTURN_LEFT);
insertTask(taskGO_FORWARD);
curOrient = orEAST;
break;
case APath.instGO_SOUTH:
insertTask(taskGO_FORWARD);
break;
case APath.instGO_WEST:
insertTask(taskTURN_RIGHT);
insertTask(taskGO_FORWARD);
curOrient = orWEST;
break;
}
break;
case orWEST:
switch (path.amoves[i])
{
case APath.instGO_NORTH:
insertTask(taskTURN_RIGHT);
insertTask(taskGO_FORWARD);
curOrient = orNORTH;
break;
case APath.instGO_EAST:
insertTask(taskGO_BACKWARD);
break;
case APath.instGO_SOUTH:
insertTask(taskTURN_LEFT);
insertTask(taskGO_FORWARD);
curOrient = orSOUTH;
break;
case APath.instGO_WEST:
insertTask(taskGO_FORWARD);
break;
}
break;
}
insertTask(taskHUG);
}