/*********************************************************************************************************************
solveMazeIt
This function solves a maze using iteration. For details on operation and maze, see above.
*********************************************************************************************************************/
void solveMazeIt(int row, int col) {
int dir = 2; // 0 is up, 1 is right, 2 is down, 3 is left.
maze[row][col] = 2; // drop a bread crumb in the starting square
while (row < MATRIX_SIZE - 1) { // the exit is the only open square
// in the last row
int potentialRow, potentialCol;
adjacentCell(row, col, dir, &potentialRow, &potentialCol);
if (potentialRow < 0)
{
dir = turnLeft(dir);
adjacentCell(row, col, dir, &potentialRow, &potentialCol);
}
if (isOK(potentialRow, potentialCol))
{
if (maze[potentialRow][potentialCol] == 2) // Backtracking
{
maze[row][col] = 0;
}
row = potentialRow;
col = potentialCol;
maze[row][col] = 2;
dir = turnRight(dir);
}
else
{
dir = turnLeft(dir);
}
}
return;
}
void growObstaclesOgMap( hydroogmap::OgMap &ogMap,
const double traversabilityThreshhold,
const int robotDiameterCells )
{
Cell2DVector Lcurr;
Cell2D q, q1;
// efficient for large radii
// find all the edges of the known obstacles
for ( int i=0; i<ogMap.numCellsX(); i++ )
{
for ( int j=0; j<ogMap.numCellsY(); j++ )
{
if ( !isTraversable( ogMap, i,j, traversabilityThreshhold ) &&
isTraversableNeighbors( ogMap, i,j, traversabilityThreshhold ) )
{
assert( ogMap.cellWithinMap( i,j ) );
Lcurr.push_back( Cell2D( i,j ) );
}
}
}
// extend them by robotDiameterCells cells
for( int i=0; i<robotDiameterCells; i++ )
{
Cell2DVector Lnext;
//cout << "Lcurr size: " << Lcurr.size()<<endl;
while ( !Lcurr.empty() )
{
q = Lcurr.back();
//cout << "Q: " << q << endl;
Lcurr.pop_back();
for ( int k=0; k<4; k++ )
{
q1 = adjacentCell(q, k);
// checking the boundaries
if ( ogMap.cellWithinMap( q1.x(), q1.y() ) )
{
const int thresh = (int)floor( (double)hydroogmap::CELL_OCCUPIED*traversabilityThreshhold );
if ( ogMap.gridCell( q1.x(), q1.y() ) < thresh )
{
ogMap.gridCell( q1.x(), q1.y() ) = hydroogmap::CELL_OCCUPIED;
Lnext.push_back( q1 );
}
}
}
}
Lcurr = Lnext;
}
}