void PMDistanceControlPoint::snapToGrid( )
{
double d = moveGrid( );
if( !approxZero( d ) )
m_distance = rint( m_distance / d ) * d;
setChanged( );
}
void USObstacleGridProvider::update(USObstacleGrid& usObstacleGrid, const OdometryData& theOdometryData, const GameInfo& theGameInfo,
const RobotInfo& theRobotInfo, const MotionRequest& theMotionRequest,
const FrameInfo& theFrameInfo, const RobotPose& theRobotPose, const FilteredSensorData& theFilteredSensorData)
{
if(!initialized)
{
lastOdometry = theOdometryData;
cells = usObstacleGrid.cells;
for(int i = 0; i < GRID_SIZE; ++i)
cells[i].state = USObstacleGrid::Cell::FREE;
initialized = true;
}
else if((gameInfoGameStateLastFrame == STATE_SET) && (theGameInfo.state == STATE_PLAYING))
{
for(int i = 0; i < GRID_SIZE; ++i)
cells[i].state = USObstacleGrid::Cell::FREE;
}
//MODIFY("parameters:USObstacleGridProvider", parameters);
//DECLARE_DEBUG_DRAWING("module:USObstacleGridProvider:us", "drawingOnField");
ageCellState(theFrameInfo);
if((theRobotInfo.penalty == PENALTY_NONE) &&
(theMotionRequest.motion != MotionRequest::specialAction) &&
(theFrameInfo.getTimeSince(lastTimePenalized) > 3000))
{
moveGrid(theOdometryData);
checkUS(theFilteredSensorData, theFrameInfo, theOdometryData);
}
else if(theRobotInfo.penalty != PENALTY_NONE)
{
lastTimePenalized = theFrameInfo.time;
}
usObstacleGrid.drawingOrigin = theRobotPose;
usObstacleGrid.drawingOrigin.rotation = normalize(usObstacleGrid.drawingOrigin.rotation - accumulatedOdometry.rotation);
usObstacleGrid.cellOccupiedThreshold = parameters.cellOccupiedThreshold;
usObstacleGrid.cellMaxOccupancy = parameters.cellMaxOccupancy;
gameInfoGameStateLastFrame = theGameInfo.state;
}
