diff --git a/modules/simulator/src/Block.cpp b/modules/simulator/src/Block.cpp index 9b32d2ac..d57700fe 100644 --- a/modules/simulator/src/Block.cpp +++ b/modules/simulator/src/Block.cpp @@ -183,7 +183,7 @@ Block::Ptr Block::factory(World* parent, const rapidxml::xml_node* root) if (block->b2dBody_) { // Init pos: - const auto q = block->getPose(); + const auto q = parent->applyWorldRenderOffset(block->getPose()); const auto dq = block->getTwist(); block->b2dBody_->SetTransform(b2Vec2(q.x, q.y), q.yaw); diff --git a/modules/simulator/src/Simulable.cpp b/modules/simulator/src/Simulable.cpp index ad016dce..d932a151 100644 --- a/modules/simulator/src/Simulable.cpp +++ b/modules/simulator/src/Simulable.cpp @@ -50,7 +50,8 @@ void Simulable::simul_pre_timestep( // if (!b2dBody_) return; // Pos: - b2dBody_->SetTransform(b2Vec2(q_.x, q_.y), q_.yaw); + const auto qq = simulable_parent_->applyWorldRenderOffset(q_); + b2dBody_->SetTransform(b2Vec2(qq.x, qq.y), q_.yaw); // Vel: b2dBody_->SetLinearVelocity(b2Vec2(dq_.vx, dq_.vy)); @@ -68,8 +69,9 @@ void Simulable::simul_post_timestep(const TSimulContext& context) // Pos: const b2Vec2& pos = b2dBody_->GetPosition(); const float angle = b2dBody_->GetAngle(); - q_.x = pos(0); - q_.y = pos(1); + const auto off = simulable_parent_->worldRenderOffset(); + q_.x = pos(0) - off.x; + q_.y = pos(1) - off.y; q_.yaw = angle; // The rest (z,pitch,roll) will be always 0, unless other // world-element modifies them! (e.g. elevation map) diff --git a/modules/simulator/src/VehicleBase.cpp b/modules/simulator/src/VehicleBase.cpp index 07af0760..871eb2c4 100644 --- a/modules/simulator/src/VehicleBase.cpp +++ b/modules/simulator/src/VehicleBase.cpp @@ -294,7 +294,7 @@ VehicleBase::Ptr VehicleBase::factory(World* parent, const rapidxml::xml_nodeb2dBody_) { // Init pos: - const auto q = veh->getPose(); + const auto q = parent->applyWorldRenderOffset(veh->getPose()); const auto dq = veh->getTwist(); veh->b2dBody_->SetTransform(b2Vec2(q.x, q.y), q.yaw); @@ -803,8 +803,8 @@ void VehicleBase::apply_force( const mrpt::math::TVector2D& force, const mrpt::math::TPoint2D& applyPoint) { ASSERT_(b2dBody_); - const b2Vec2 wPt = b2dBody_->GetWorldPoint( - b2Vec2(applyPoint.x, applyPoint.y)); // Application point -> world coords + // Application point -> world coords + const b2Vec2 wPt = b2dBody_->GetWorldPoint(b2Vec2(applyPoint.x, applyPoint.y)); b2dBody_->ApplyForce(b2Vec2(force.x, force.y), wPt, true /*wake up*/); }