/*********************************************************************************************************************
                                          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;
}
Beispiel #2
0
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;
    }
}