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; }