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