Fix loss of precision in box2d due to float in case of UTM coordinates

MRPT · Nov 29, 2024 · fbb8cbf · fbb8cbf
1 parent 6fb9b40
commit fbb8cbf
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 7 deletions.
diff --git 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<char>* 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
@@ -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
@@ -294,7 +294,7 @@ VehicleBase::Ptr VehicleBase::factory(World* parent, const rapidxml::xml_node<ch
 	if (veh->b2dBody_)
 	{
 		// 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*/);
 }