Support UTM coords

modules/simulator/include/mvsim/Block.h

@@ -137,7 +137,7 @@ class Block : public VisualObject, public Simulable
 
 	VisualObject* meAsVisualObject() override { return this; }
 
-	std::optional<float> getElevationAt(const mrpt::math::TPoint2Df& worldXY) const override;
+	std::optional<float> getElevationAt(const mrpt::math::TPoint2D& worldXY) const override;
 
    protected:
 	virtual void internalGuiUpdate(

modules/simulator/include/mvsim/Simulable.h

@@ -135,7 +135,7 @@ class Simulable
 	 *  for the queried (x,y). If the coordinates do not affect this object, it will return nullopt.
 	 */
 	virtual std::optional<float> getElevationAt(
-		[[maybe_unused]] const mrpt::math::TPoint2Df& worldXY) const
+		[[maybe_unused]] const mrpt::math::TPoint2D& worldXY) const
 	{
 		return std::nullopt;
 	}

modules/simulator/include/mvsim/World.h

@@ -402,7 +402,7 @@ class World : public mrpt::system::COutputLogger
 	 * always reported by default. In multistorey worlds, for example, this will return the height
 	 * of each floor for the queried point.
 	 */
-	std::set<float> getElevationsAt(const mrpt::math::TPoint2Df& worldXY) const;
+	std::set<float> getElevationsAt(const mrpt::math::TPoint2D& worldXY) const;
 
 	/// with query points the center of a wheel, this returns the highest "ground" under it, or .0
 	/// if nothing found.
@@ -607,6 +607,33 @@ class World : public mrpt::system::COutputLogger
 
 	const GeoreferenceOptions& georeferenceOptions() const { return georeferenceOptions_; }
 
+	/// (See docs for worldRenderOffset_)
+	mrpt::math::TVector3D worldRenderOffset() const
+	{
+		return worldRenderOffset_ ? *worldRenderOffset_ : mrpt::math::TVector3D(0, 0, 0);
+	}
+	mrpt::math::TPose3D applyWorldRenderOffset(mrpt::math::TPose3D p) const
+	{
+		const auto t = worldRenderOffset();
+		p.x += t.x;
+		p.y += t.y;
+		p.z += t.z;
+		return p;
+	}
+	mrpt::poses::CPose3D applyWorldRenderOffset(mrpt::poses::CPose3D p) const
+	{
+		const auto t = worldRenderOffset();
+		p.x_incr(t.x);
+		p.y_incr(t.y);
+		p.z_incr(t.z);
+		return p;
+	}
+	/// (See docs for worldRenderOffset_)
+	void worldRenderOffsetPropose(const mrpt::math::TVector3D& v)
+	{
+		if (!worldRenderOffset_) worldRenderOffset_ = v;
+	}
+
    private:
 	/** Options for lights */
 	GeoreferenceOptions georeferenceOptions_;
@@ -731,6 +758,13 @@ class World : public mrpt::system::COutputLogger
 	mrpt::opengl::COpenGLScene worldPhysical_;
 	std::recursive_mutex worldPhysicalMtx_;
 
+	/// World coordinates offset for rendering. Useful mainly to keep numerical accuracy
+	/// in the OpenGL pipeline (using "floats") when using UTM world coordinates.
+	/// All coordinates to be send to OpenGL must **add** this number.
+	/// It is automatically set via calling worldRenderOffsetPropose()
+	/// and must be retrieved via worldRenderOffset()
+	std::optional<mrpt::math::TVector3D> worldRenderOffset_;
+
 	/// Updated in internal_one_step()
 	std::map<std::string, mrpt::math::TPose3D> copy_of_objects_dynstate_pose_;
 	std::map<std::string, mrpt::math::TTwist2D> copy_of_objects_dynstate_twist_;

modules/simulator/include/mvsim/WorldElements/ElevationMap.h

@@ -36,7 +36,7 @@ class ElevationMap : public WorldElementBase
 	virtual void simul_pre_timestep(const TSimulContext& context) override;
 	virtual void simul_post_timestep(const TSimulContext& context) override;
 
-	std::optional<float> getElevationAt(const mrpt::math::TPoint2Df& pt) const override;
+	std::optional<float> getElevationAt(const mrpt::math::TPoint2D& pt) const override;
 
    protected:
 	virtual void internalGuiUpdate(
@@ -47,17 +47,17 @@ class ElevationMap : public WorldElementBase
 	 */
 	std::vector<mrpt::opengl::CMesh::Ptr> gl_meshes_;
 	bool firstSceneRendering_ = true;
-	float resolution_ = 1.0f;
+	double resolution_ = 1.0f;
 
-	float textureExtensionX_ = 0;  //!< 0=auto
-	float textureExtensionY_ = 0;  //!< 0=auto
+	double textureExtensionX_ = 0;	//!< 0=auto
+	double textureExtensionY_ = 0;	//!< 0=auto
 
 	/** A copy of elevation data in gl_mesh_. Coordinate order is (x,y) */
-	mrpt::math::CMatrixFloat meshCacheZ_;
-	float meshMinX_ = 0, meshMaxX_ = 0, meshMinY_ = 0, meshMaxY_ = 0;
+	mrpt::math::CMatrixDouble meshCacheZ_;
+	double meshMinX_ = 0, meshMaxX_ = 0, meshMinY_ = 0, meshMaxY_ = 0;
 
 	/// If enabled (>0), the mesh will be split into NxM smaller meshes with a max size of this
 	/// value, to help correctly render semitransparent objects (e.g. trees).
-	float model_split_size_ = .0f;
+	double model_split_size_ = .0f;
 };
 }  // namespace mvsim

modules/simulator/include/mvsim/WorldElements/HorizontalPlane.h

@@ -32,7 +32,7 @@ class HorizontalPlane : public WorldElementBase
 	void simul_pre_timestep(const TSimulContext& context) override;
 	void simul_post_timestep(const TSimulContext& context) override;
 
-	std::optional<float> getElevationAt(const mrpt::math::TPoint2Df& worldXY) const override;
+	std::optional<float> getElevationAt(const mrpt::math::TPoint2D& worldXY) const override;
 
    protected:
 	virtual void internalGuiUpdate(

modules/simulator/src/Block.cpp

@@ -265,7 +265,7 @@ void Block::internalGuiUpdate(
 		// mutex and we don't need/can't acquire it again:
 		const auto objectPose = viz.has_value() ? getPose() : getPoseNoLock();
 
-		if (gl_block_) gl_block_->setPose(objectPose);
+		if (gl_block_) gl_block_->setPose(parent()->applyWorldRenderOffset(objectPose));
 	}
 
 	if (!gl_forces_ && viz)
@@ -275,6 +275,8 @@ void Block::internalGuiUpdate(
 		gl_forces_->setLineWidth(3.0);
 		gl_forces_->setColor_u8(0xff, 0xff, 0xff);
 
+		gl_forces_->setPose(parent()->applyWorldRenderOffset(mrpt::poses::CPose3D::Identity()));
+
 		viz->get().insert(gl_forces_);	// forces are in global coords
 	}
 
@@ -592,7 +594,7 @@ bool Block::default_block_z_min_max() const
 	return block_z_max_ != block_z_max_ || block_z_min_ != block_z_min_;
 }
 
-std::optional<float> mvsim::Block::getElevationAt(const mrpt::math::TPoint2Df& worldXY) const
+std::optional<float> mvsim::Block::getElevationAt(const mrpt::math::TPoint2D& worldXY) const
 {
 	// Is the point within the block?
 	const auto& myPose = getCPose3D();

modules/simulator/src/Sensors/CameraSensor.cpp

@@ -141,12 +141,12 @@ void CameraSensor::internalGuiUpdate(
 
 	// Move with vehicle:
 	const auto& p = vehicle_.getPose();
+	const auto pp = parent()->applyWorldRenderOffset(p);
 
-	gl_sensor_fov_->setPose(p);
-	gl_sensor_origin_->setPose(p);
-
-	const auto globalSensorPose = p + sensor_params_.cameraPose.asTPose();
+	gl_sensor_fov_->setPose(pp);
+	gl_sensor_origin_->setPose(pp);
 
+	const auto globalSensorPose = pp + sensor_params_.cameraPose.asTPose();
 	if (glCustomVisual_) glCustomVisual_->setPose(globalSensorPose);
 }
 
@@ -204,7 +204,7 @@ void CameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 	const auto vehiclePose = mrpt::poses::CPose3D(vehicle_.getPose());
 	const auto rgbSensorPose = vehiclePose + curObs->cameraPose;
 
-	cam.setPose(rgbSensorPose);
+	cam.setPose(world()->applyWorldRenderOffset(rgbSensorPose));
 
 	ASSERT_(fbo_renderer_rgb_);
 

modules/simulator/src/Sensors/DepthCameraSensor.cpp

@@ -192,11 +192,13 @@ void DepthCameraSensor::internalGuiUpdate(
 	// Move with vehicle:
 	const auto& p = vehicle_.getPose();
 
-	if (gl_obs_) gl_obs_->setPose(p);
-	if (gl_sensor_fov_) gl_sensor_fov_->setPose(p);
-	if (gl_sensor_origin_) gl_sensor_origin_->setPose(p);
+	const auto pp = parent()->applyWorldRenderOffset(p);
 
-	if (glCustomVisual_) glCustomVisual_->setPose(p + sensor_params_.sensorPose.asTPose());
+	if (gl_obs_) gl_obs_->setPose(pp);
+	if (gl_sensor_fov_) gl_sensor_fov_->setPose(pp);
+	if (gl_sensor_origin_) gl_sensor_origin_->setPose(pp);
+
+	if (glCustomVisual_) glCustomVisual_->setPose(pp + sensor_params_.sensorPose.asTPose());
 }
 
 void DepthCameraSensor::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {}
@@ -284,7 +286,7 @@ void DepthCameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DSce
 
 		camRGB->set6DOFMode(true);
 		camRGB->setProjectiveFromPinhole(curObs.cameraParamsIntensity);
-		camRGB->setPose(rgbSensorPose);
+		camRGB->setPose(world()->applyWorldRenderOffset(rgbSensorPose));
 
 		// viewport->setCustomBackgroundColor({0.3f, 0.3f, 0.3f, 1.0f});
 		viewport->setViewportClipDistances(rgbClipMin_, rgbClipMax_);
@@ -311,7 +313,7 @@ void DepthCameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DSce
 		// Note: relativePoseOnVehicle should be (y,p,r)=(90deg,0,90deg) to make
 		// the camera to look forward:
 		camDepth->set6DOFMode(true);
-		camDepth->setPose(depthSensorPose);
+		camDepth->setPose(world()->applyWorldRenderOffset(depthSensorPose));
 
 		// viewport->setCustomBackgroundColor({0.3f, 0.3f, 0.3f, 1.0f});
 		viewport->setViewportClipDistances(depth_clip_min_, depth_clip_max_);

modules/simulator/src/Sensors/GNSS.cpp

@@ -79,9 +79,10 @@ void GNSS::internalGuiUpdate(
 	}
 
 	const mrpt::poses::CPose3D p = vehicle_.getCPose3D() + obs_model_.sensorPose;
+	const auto pp = parent()->applyWorldRenderOffset(p);
 
-	if (gl_sensor_origin_) gl_sensor_origin_->setPose(p);
-	if (glCustomVisual_) glCustomVisual_->setPose(p);
+	if (gl_sensor_origin_) gl_sensor_origin_->setPose(pp);
+	if (glCustomVisual_) glCustomVisual_->setPose(pp);
 }
 
 void GNSS::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {}

modules/simulator/src/Sensors/IMU.cpp

@@ -71,9 +71,10 @@ void IMU::internalGuiUpdate(
 	}
 
 	const mrpt::poses::CPose3D p = vehicle_.getCPose3D() + obs_model_.sensorPose;
+	const auto pp = parent()->applyWorldRenderOffset(p);
 
-	if (gl_sensor_origin_) gl_sensor_origin_->setPose(p);
-	if (glCustomVisual_) glCustomVisual_->setPose(p);
+	if (gl_sensor_origin_) gl_sensor_origin_->setPose(pp);
+	if (glCustomVisual_) glCustomVisual_->setPose(pp);
 }
 
 void IMU::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {}

modules/simulator/src/Sensors/LaserScanner.cpp

@@ -175,20 +175,21 @@ void LaserScanner::internalGuiUpdate(
 	}
 
 	const mrpt::poses::CPose3D p = vehicle_.getCPose3D();
+	const auto pp = parent()->applyWorldRenderOffset(p);
 
 	const double z_incrs = 1e-3;  // for z_order_
 	const double z_offset = 1e-3;
 
 	if (gl_scan_)
 	{
-		auto p2 = p;
+		auto p2 = pp;
 		p2.z_incr(z_offset + z_incrs * z_order_);
 		gl_scan_->setPose(p2);
 	}
 
-	if (gl_sensor_fov_) gl_sensor_fov_->setPose(p);
-	if (gl_sensor_origin_) gl_sensor_origin_->setPose(p);
-	if (glCustomVisual_) glCustomVisual_->setPose(p + scan_model_.sensorPose);
+	if (gl_sensor_fov_) gl_sensor_fov_->setPose(pp);
+	if (gl_sensor_origin_) gl_sensor_origin_->setPose(pp);
+	if (glCustomVisual_) glCustomVisual_->setPose(pp + scan_model_.sensorPose);
 }
 
 void LaserScanner::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {}
@@ -581,7 +582,7 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 		// Camera pose: vehicle + relativePoseOnVehicle:
 		// Note: relativePoseOnVehicle should be (y,p,r)=(90deg,0,90deg) to make
 		// the camera to look forward:
-		cam.setPose(depthSensorPose);
+		cam.setPose(world()->applyWorldRenderOffset(depthSensorPose));
 
 		auto tleRender =
 			mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.2Dlidar.renderSubScan");

modules/simulator/src/Sensors/Lidar3D.cpp

@@ -144,11 +144,12 @@ void Lidar3D::internalGuiUpdate(
 	}
 
 	const mrpt::poses::CPose3D p = vehicle_.getCPose3D() + sensorPoseOnVeh_;
+	const auto pp = parent()->applyWorldRenderOffset(p);
 
-	if (glPoints_) glPoints_->setPose(p);
-	if (gl_sensor_fov_) gl_sensor_fov_->setPose(p);
-	if (gl_sensor_origin_) gl_sensor_origin_->setPose(p);
-	if (glCustomVisual_) glCustomVisual_->setPose(p);
+	if (glPoints_) glPoints_->setPose(pp);
+	if (gl_sensor_fov_) gl_sensor_fov_->setPose(pp);
+	if (gl_sensor_origin_) gl_sensor_origin_->setPose(pp);
+	if (glCustomVisual_) glCustomVisual_->setPose(pp);
 }
 
 void Lidar3D::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {}
@@ -528,7 +529,7 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene)
 		// Camera pose: vehicle + relativePoseOnVehicle:
 		// Note: relativePoseOnVehicle should be (y,p,r)=(90deg,0,90deg)
 		// to make the camera to look forward:
-		cam.setPose(thisDepthSensorPose);
+		cam.setPose(world()->applyWorldRenderOffset(thisDepthSensorPose));
 
 		auto tleRender =
 			mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.3Dlidar.renderSubScan");

modules/simulator/src/VehicleBase.cpp

@@ -715,11 +715,12 @@ void VehicleBase::internalGuiUpdate(
 	// setPose() change event, so my caller already holds the mutex and we don't
 	// need/can't acquire it again:
 	const auto objectPose = viz.has_value() ? getPose() : getPoseNoLock();
+	const auto pp = parent()->applyWorldRenderOffset(objectPose);
 
 	if (glInit_)
 	{
-		glChassisViz_->setPose(objectPose);
-		glChassisPhysical_->setPose(objectPose);
+		glChassisViz_->setPose(pp);
+		glChassisPhysical_->setPose(pp);
 		for (size_t i = 0; i < nWs; i++)
 		{
 			const Wheel& w = getWheelInfo(i);
@@ -753,6 +754,8 @@ void VehicleBase::internalGuiUpdate(
 		glForces_ = mrpt::opengl::CSetOfLines::Create();
 		glForces_->setLineWidth(3.0);
 		glForces_->setColor_u8(0xff, 0xff, 0xff);
+		glForces_->setPose(parent()->applyWorldRenderOffset(mrpt::poses::CPose3D::Identity()));
+
 		viz->get().insert(glForces_);  // forces are in global coords
 	}
 	if (!glMotorTorques_ && viz)
@@ -761,6 +764,8 @@ void VehicleBase::internalGuiUpdate(
 		glMotorTorques_ = mrpt::opengl::CSetOfLines::Create();
 		glMotorTorques_->setLineWidth(3.0);
 		glMotorTorques_->setColor_u8(0xff, 0x00, 0x00);
+		glMotorTorques_->setPose(
+			parent()->applyWorldRenderOffset(mrpt::poses::CPose3D::Identity()));
 		viz->get().insert(glMotorTorques_);	 // torques are in global coords
 	}
 

modules/simulator/src/VisualObject.cpp

@@ -53,7 +53,8 @@ void VisualObject::guiUpdate(
 	// If "viz" does not have a value, it's because we are already inside a
 	// setPose() change event, so my caller already holds the mutex and we don't
 	// need/can't acquire it again:
-	const auto objectPose = viz.has_value() ? meSim->getPose() : meSim->getPoseNoLock();
+	const auto objectPoseOrg = viz.has_value() ? meSim->getPose() : meSim->getPoseNoLock();
+	const auto objectPose = parent()->applyWorldRenderOffset(objectPoseOrg);
 
 	if (glCustomVisual_ && viz.has_value() && physical.has_value())
 	{

modules/simulator/src/World.cpp

@@ -272,7 +272,7 @@ void World::internalOnObservation(const Simulable& veh, const mrpt::obs::CObserv
 	arch << *obs;
 }
 
-std::set<float> World::getElevationsAt(const mrpt::math::TPoint2Df& worldXY) const
+std::set<float> World::getElevationsAt(const mrpt::math::TPoint2D& worldXY) const
 {
 	// Assumption: getListOfSimulableObjectsMtx() is already adquired by all possible call paths?
 	std::set<float> ret;