bool isTraversableNeighbors( const hydroogmap::OgMap &ogMap,
const int indX,
const int indY,
const float traversabilityThreshhold )
{
return ( isTraversable(ogMap,indX+1,indY,traversabilityThreshhold) ||
isTraversable(ogMap,indX-1,indY,traversabilityThreshhold) ||
isTraversable(ogMap,indX,indY+1,traversabilityThreshhold) ||
isTraversable(ogMap,indX,indY-1,traversabilityThreshhold) );
}
MSWidgetView *AplusPopup::getNextFocusAfter(MSWidgetView *pWidgetView_)
{
if (traversalList().length()>0)
{
MSWidget *pWidget=0;
unsigned i,index=traversalList().indexOf((unsigned long)pWidgetView_,0);
if (index==traversalList().length()) index=0;
for (i=index+1;i<traversalList().length();i++)
{
pWidget=traversalList()(i);
if (isTraversable(pWidget)==MSTrue) return (MSWidgetView *)pWidget;
}
for (i=0;i<index;i++)
{
pWidget=traversalList()(i);
if (isTraversable(pWidget)==MSTrue) return (MSWidgetView *)pWidget;
}
}
return 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;
}
}
