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; }
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<int> USObstacleGridProvider::gridToWorld(const Vector2<int>& p, const OdometryData& theOdometryData) const { Vector2<> pFloat((float) p.x, (float) p.y); const float move(GRID_LENGTH / 2); pFloat -= Vector2<>(move, move); pFloat *= CELL_SIZE; Pose2D odoRotation(-theOdometryData.rotation, 0.0f, 0.0f); pFloat = odoRotation * pFloat; return Vector2<int>(static_cast<int>(pFloat.x), static_cast<int>(pFloat.y)); }