inline Vector2<> ObstacleCombinator::gridToWorld(const Vector2<>& p) const { Vector2<> pWorld(p); const float move(USObstacleGrid::GRID_LENGTH / 2); pWorld -= Vector2<>(move, move); pWorld *= USObstacleGrid::CELL_SIZE; Pose2D odoRotation(-theOdometryData.rotation, 0.0f, 0.0f); return odoRotation * pWorld; }
inline Vector2<> USObstacleGridProvider::gridToWorld(const Vector2<>& p,const OdometryData& theOdometryData) const { Vector2<> pWorld(p); const float move(GRID_LENGTH / 2); pWorld -= Vector2<>(move, move); pWorld *= CELL_SIZE; Pose2D odoRotation(-theOdometryData.rotation, 0.0f, 0.0f); return odoRotation * pWorld; }