From 2d823b6d07f035c799041c9310097f563817aab9 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 30 Dec 2024 00:19:48 +0100 Subject: [PATCH] WIP: new mrpt-viz library --- libs/opengl/include/mrpt/opengl/CPointCloud.h | 10 - .../opengl/CRenderizableShaderTriangles.h | 17 - .../opengl/CRenderizableShaderWireFrame.h | 16 - libs/opengl/src/CPointCloud.cpp | 2 - libs/opengl/src/CRenderizableShaderPoints.cpp | 15 - .../CRenderizableShaderTexturedTriangles.cpp | 148 -- .../src/CRenderizableShaderTriangles.cpp | 20 - libs/viz/CMakeLists.txt | 19 + libs/viz/include/mrpt/viz/CArrow.h | 127 + libs/viz/include/mrpt/viz/CAssimpModel.h | 106 + libs/viz/include/mrpt/viz/CAxis.h | 75 + libs/viz/include/mrpt/viz/CBox.h | 101 + libs/viz/include/mrpt/viz/CCamera.h | 185 ++ libs/viz/include/mrpt/viz/CColorBar.h | 60 + libs/viz/include/mrpt/viz/CCylinder.h | 170 ++ libs/viz/include/mrpt/viz/CDisk.h | 70 + libs/viz/include/mrpt/viz/CEllipsoid2D.h | 60 + libs/viz/include/mrpt/viz/CEllipsoid3D.h | 59 + .../mrpt/viz/CEllipsoidInverseDepth2D.h | 65 + .../mrpt/viz/CEllipsoidInverseDepth3D.h | 73 + .../mrpt/viz/CEllipsoidRangeBearing2D.h | 57 + libs/viz/include/mrpt/viz/CFrustum.h | 126 + .../mrpt/viz/CGeneralizedEllipsoidTemplate.h | 278 +++ libs/viz/include/mrpt/viz/CGridPlaneXY.h | 82 + libs/viz/include/mrpt/viz/CGridPlaneXZ.h | 81 + libs/viz/include/mrpt/viz/CMesh.h | 260 ++ libs/viz/include/mrpt/viz/CMesh3D.h | 141 ++ libs/viz/include/mrpt/viz/CMeshFast.h | 212 ++ libs/viz/include/mrpt/viz/COctoMapVoxels.h | 336 +++ libs/viz/include/mrpt/viz/CPointCloud.h | 362 +++ .../include/mrpt/viz/CPointCloudColoured.h | 402 ++++ libs/viz/include/mrpt/viz/CPolyhedron.h | 928 ++++++++ libs/viz/include/mrpt/viz/CSetOfLines.h | 219 ++ libs/viz/include/mrpt/viz/CSetOfObjects.h | 184 ++ .../mrpt/viz/CSetOfTexturedTriangles.h | 76 + libs/viz/include/mrpt/viz/CSetOfTriangles.h | 144 ++ libs/viz/include/mrpt/viz/CSimpleLine.h | 84 + libs/viz/include/mrpt/viz/CSkyBox.h | 55 + libs/viz/include/mrpt/viz/CSphere.h | 71 + libs/viz/include/mrpt/viz/CText.h | 70 + libs/viz/include/mrpt/viz/CText3D.h | 107 + .../include/mrpt/viz/CTextMessageCapable.h | 86 + libs/viz/include/mrpt/viz/CTexturedPlane.h | 79 + libs/viz/include/mrpt/viz/CUBE_TEXTURE_FACE.h | 37 + libs/viz/include/mrpt/viz/CVectorField2D.h | 195 ++ libs/viz/include/mrpt/viz/CVectorField3D.h | 312 +++ libs/viz/include/mrpt/viz/CVisualObject.h | 666 ++++++ .../include/mrpt/viz}/PLY_import_export.h | 6 +- libs/viz/include/mrpt/viz/Scene.h | 250 ++ libs/viz/include/mrpt/viz/TLightParameters.h | 67 + libs/viz/include/mrpt/viz/TTriangle.h | 146 ++ libs/viz/include/mrpt/viz/Viewport.h | 422 ++++ libs/viz/include/mrpt/viz/Visualizable.h | 43 + libs/viz/include/mrpt/viz/graph_tools.h | 69 + libs/viz/include/mrpt/viz/graph_tools_impl.h | 195 ++ .../include/mrpt/viz}/opengl_fonts.h | 4 +- .../include/mrpt/viz/pointcloud_adapters.h | 43 + libs/viz/include/mrpt/viz/pose_pdfs.h | 27 + .../viz/include/mrpt/viz/registerAllClasses.h | 20 + libs/viz/include/mrpt/viz/stock_objects.h | 176 ++ libs/viz/include/mrpt/viz/viz_frwds.h | 49 + libs/viz/src/CArrow.cpp | 109 + libs/viz/src/CAssimpModel.cpp | 85 + libs/viz/src/CAxis.cpp | 175 ++ libs/viz/src/CBox.cpp | 101 + libs/viz/src/CCamera.cpp | 93 + libs/viz/src/CColorBar.cpp | 105 + libs/viz/src/CCylinder.cpp | 210 ++ libs/viz/src/CDisk.cpp | 93 + libs/viz/src/CEllipsoid2D.cpp | 119 + libs/viz/src/CEllipsoid3D.cpp | 120 + libs/viz/src/CEllipsoidInverseDepth2D.cpp | 74 + libs/viz/src/CEllipsoidInverseDepth3D.cpp | 76 + libs/viz/src/CEllipsoidRangeBearing2D.cpp | 66 + libs/viz/src/CFrustum.cpp | 197 ++ .../viz/src/CGeneralizedEllipsoidTemplate.cpp | 111 + libs/viz/src/CGridPlaneXY.cpp | 79 + libs/viz/src/CGridPlaneXZ.cpp | 79 + libs/viz/src/CMesh.cpp | 548 +++++ libs/viz/src/CMesh3D.cpp | 229 ++ libs/viz/src/CMeshFast.cpp | 219 ++ libs/viz/src/COctoMapVoxels.cpp | 153 ++ libs/viz/src/CPointCloud.cpp | 305 +++ libs/viz/src/CPointCloudColoured.cpp | 209 ++ libs/viz/src/CPolyhedron.cpp | 2120 +++++++++++++++++ libs/viz/src/CSetOfLines.cpp | 143 ++ libs/viz/src/CSetOfObjects.cpp | 234 ++ libs/viz/src/CSetOfTexturedTriangles.cpp | 81 + libs/viz/src/CSetOfTriangles.cpp | 151 ++ libs/viz/src/CSimpleLine.cpp | 78 + libs/viz/src/CSkyBox.cpp | 70 + libs/viz/src/CSphere.cpp | 98 + libs/viz/src/CText.cpp | 92 + libs/viz/src/CText3D.cpp | 77 + libs/viz/src/CTextMessageCapable.cpp | 84 + libs/viz/src/CTexturedPlane.cpp | 87 + libs/viz/src/CVectorField2D.cpp | 108 + libs/viz/src/CVectorField3D.cpp | 91 + libs/viz/src/CVisualObject.cpp | 530 +++++ .../{opengl => viz}/src/PLY_import_export.cpp | 6 +- libs/viz/src/Scene.cpp | 295 +++ libs/viz/src/StockObjects.cpp | 507 ++++ libs/viz/src/TLightParameters.cpp | 73 + libs/viz/src/TTriangle.cpp | 49 + libs/viz/src/Viewport.cpp | 512 ++++ libs/viz/src/Visualizable.cpp | 22 + libs/viz/src/pose_pdfs.cpp | 309 +++ libs/viz/src/registerAllClasses.cpp | 127 + libs/viz/src/serializations_unittest.cpp | 164 ++ libs/viz/src/viz-precomp.cpp | 10 + libs/viz/src/viz-precomp.h | 16 + 111 files changed, 18018 insertions(+), 236 deletions(-) create mode 100644 libs/viz/CMakeLists.txt create mode 100644 libs/viz/include/mrpt/viz/CArrow.h create mode 100644 libs/viz/include/mrpt/viz/CAssimpModel.h create mode 100644 libs/viz/include/mrpt/viz/CAxis.h create mode 100644 libs/viz/include/mrpt/viz/CBox.h create mode 100644 libs/viz/include/mrpt/viz/CCamera.h create mode 100644 libs/viz/include/mrpt/viz/CColorBar.h create mode 100644 libs/viz/include/mrpt/viz/CCylinder.h create mode 100644 libs/viz/include/mrpt/viz/CDisk.h create mode 100644 libs/viz/include/mrpt/viz/CEllipsoid2D.h create mode 100644 libs/viz/include/mrpt/viz/CEllipsoid3D.h create mode 100644 libs/viz/include/mrpt/viz/CEllipsoidInverseDepth2D.h create mode 100644 libs/viz/include/mrpt/viz/CEllipsoidInverseDepth3D.h create mode 100644 libs/viz/include/mrpt/viz/CEllipsoidRangeBearing2D.h create mode 100644 libs/viz/include/mrpt/viz/CFrustum.h create mode 100644 libs/viz/include/mrpt/viz/CGeneralizedEllipsoidTemplate.h create mode 100644 libs/viz/include/mrpt/viz/CGridPlaneXY.h create mode 100644 libs/viz/include/mrpt/viz/CGridPlaneXZ.h create mode 100644 libs/viz/include/mrpt/viz/CMesh.h create mode 100644 libs/viz/include/mrpt/viz/CMesh3D.h create mode 100644 libs/viz/include/mrpt/viz/CMeshFast.h create mode 100644 libs/viz/include/mrpt/viz/COctoMapVoxels.h create mode 100644 libs/viz/include/mrpt/viz/CPointCloud.h create mode 100644 libs/viz/include/mrpt/viz/CPointCloudColoured.h create mode 100644 libs/viz/include/mrpt/viz/CPolyhedron.h create mode 100644 libs/viz/include/mrpt/viz/CSetOfLines.h create mode 100644 libs/viz/include/mrpt/viz/CSetOfObjects.h create mode 100644 libs/viz/include/mrpt/viz/CSetOfTexturedTriangles.h create mode 100644 libs/viz/include/mrpt/viz/CSetOfTriangles.h create mode 100644 libs/viz/include/mrpt/viz/CSimpleLine.h create mode 100644 libs/viz/include/mrpt/viz/CSkyBox.h create mode 100644 libs/viz/include/mrpt/viz/CSphere.h create mode 100644 libs/viz/include/mrpt/viz/CText.h create mode 100644 libs/viz/include/mrpt/viz/CText3D.h create mode 100644 libs/viz/include/mrpt/viz/CTextMessageCapable.h create mode 100644 libs/viz/include/mrpt/viz/CTexturedPlane.h create mode 100644 libs/viz/include/mrpt/viz/CUBE_TEXTURE_FACE.h create mode 100644 libs/viz/include/mrpt/viz/CVectorField2D.h create mode 100644 libs/viz/include/mrpt/viz/CVectorField3D.h create mode 100644 libs/viz/include/mrpt/viz/CVisualObject.h rename libs/{opengl/include/mrpt/opengl => viz/include/mrpt/viz}/PLY_import_export.h (98%) create mode 100644 libs/viz/include/mrpt/viz/Scene.h create mode 100644 libs/viz/include/mrpt/viz/TLightParameters.h create mode 100644 libs/viz/include/mrpt/viz/TTriangle.h create mode 100644 libs/viz/include/mrpt/viz/Viewport.h create mode 100644 libs/viz/include/mrpt/viz/Visualizable.h create mode 100644 libs/viz/include/mrpt/viz/graph_tools.h create mode 100644 libs/viz/include/mrpt/viz/graph_tools_impl.h rename libs/{opengl/include/mrpt/opengl => viz/include/mrpt/viz}/opengl_fonts.h (97%) create mode 100644 libs/viz/include/mrpt/viz/pointcloud_adapters.h create mode 100644 libs/viz/include/mrpt/viz/pose_pdfs.h create mode 100644 libs/viz/include/mrpt/viz/registerAllClasses.h create mode 100644 libs/viz/include/mrpt/viz/stock_objects.h create mode 100644 libs/viz/include/mrpt/viz/viz_frwds.h create mode 100644 libs/viz/src/CArrow.cpp create mode 100644 libs/viz/src/CAssimpModel.cpp create mode 100644 libs/viz/src/CAxis.cpp create mode 100644 libs/viz/src/CBox.cpp create mode 100644 libs/viz/src/CCamera.cpp create mode 100644 libs/viz/src/CColorBar.cpp create mode 100644 libs/viz/src/CCylinder.cpp create mode 100644 libs/viz/src/CDisk.cpp create mode 100644 libs/viz/src/CEllipsoid2D.cpp create mode 100644 libs/viz/src/CEllipsoid3D.cpp create mode 100644 libs/viz/src/CEllipsoidInverseDepth2D.cpp create mode 100644 libs/viz/src/CEllipsoidInverseDepth3D.cpp create mode 100644 libs/viz/src/CEllipsoidRangeBearing2D.cpp create mode 100644 libs/viz/src/CFrustum.cpp create mode 100644 libs/viz/src/CGeneralizedEllipsoidTemplate.cpp create mode 100644 libs/viz/src/CGridPlaneXY.cpp create mode 100644 libs/viz/src/CGridPlaneXZ.cpp create mode 100644 libs/viz/src/CMesh.cpp create mode 100644 libs/viz/src/CMesh3D.cpp create mode 100644 libs/viz/src/CMeshFast.cpp create mode 100644 libs/viz/src/COctoMapVoxels.cpp create mode 100644 libs/viz/src/CPointCloud.cpp create mode 100644 libs/viz/src/CPointCloudColoured.cpp create mode 100644 libs/viz/src/CPolyhedron.cpp create mode 100644 libs/viz/src/CSetOfLines.cpp create mode 100644 libs/viz/src/CSetOfObjects.cpp create mode 100644 libs/viz/src/CSetOfTexturedTriangles.cpp create mode 100644 libs/viz/src/CSetOfTriangles.cpp create mode 100644 libs/viz/src/CSimpleLine.cpp create mode 100644 libs/viz/src/CSkyBox.cpp create mode 100644 libs/viz/src/CSphere.cpp create mode 100644 libs/viz/src/CText.cpp create mode 100644 libs/viz/src/CText3D.cpp create mode 100644 libs/viz/src/CTextMessageCapable.cpp create mode 100644 libs/viz/src/CTexturedPlane.cpp create mode 100644 libs/viz/src/CVectorField2D.cpp create mode 100644 libs/viz/src/CVectorField3D.cpp create mode 100644 libs/viz/src/CVisualObject.cpp rename libs/{opengl => viz}/src/PLY_import_export.cpp (99%) create mode 100644 libs/viz/src/Scene.cpp create mode 100644 libs/viz/src/StockObjects.cpp create mode 100644 libs/viz/src/TLightParameters.cpp create mode 100644 libs/viz/src/TTriangle.cpp create mode 100644 libs/viz/src/Viewport.cpp create mode 100644 libs/viz/src/Visualizable.cpp create mode 100644 libs/viz/src/pose_pdfs.cpp create mode 100644 libs/viz/src/registerAllClasses.cpp create mode 100644 libs/viz/src/serializations_unittest.cpp create mode 100644 libs/viz/src/viz-precomp.cpp create mode 100644 libs/viz/src/viz-precomp.h diff --git a/libs/opengl/include/mrpt/opengl/CPointCloud.h b/libs/opengl/include/mrpt/opengl/CPointCloud.h index bf19b9bdb2..4b40900c64 100644 --- a/libs/opengl/include/mrpt/opengl/CPointCloud.h +++ b/libs/opengl/include/mrpt/opengl/CPointCloud.h @@ -54,9 +54,6 @@ class CPointCloud : * to have an easy to use name. */ std::vector& m_points = CRenderizableShaderPoints::m_vertex_buffer_data; - /** Default: false */ - bool m_pointSmooth = false; - mutable size_t m_last_rendered_count{0}, m_last_rendered_count_ongoing{0}; /** Do needed internal work if all points are new (octree rebuilt,...) */ @@ -285,13 +282,6 @@ class CPointCloud : CRenderizable::notifyChange(); } - void enablePointSmooth(bool enable = true) - { - m_pointSmooth = enable; - CRenderizable::notifyChange(); - } - void disablePointSmooth() { m_pointSmooth = false; } - bool isPointSmoothEnabled() const { return m_pointSmooth; } /** Sets the colors used as extremes when colorFromDepth is enabled. */ void setGradientColors(const mrpt::img::TColorf& colorMin, const mrpt::img::TColorf& colorMax); diff --git a/libs/opengl/include/mrpt/opengl/CRenderizableShaderTriangles.h b/libs/opengl/include/mrpt/opengl/CRenderizableShaderTriangles.h index b8c2655d86..d67f5907fb 100644 --- a/libs/opengl/include/mrpt/opengl/CRenderizableShaderTriangles.h +++ b/libs/opengl/include/mrpt/opengl/CRenderizableShaderTriangles.h @@ -50,17 +50,6 @@ class CRenderizableShaderTriangles : public virtual CRenderizable m_vao.destroy(); } - bool isLightEnabled() const { return m_enableLight; } - void enableLight(bool enable = true) { m_enableLight = enable; } - - /** Control whether to render the FRONT, BACK, or BOTH (default) set of - * faces. Refer to docs for glCullFace(). - * Example: If set to `cullFaces(TCullFace::BACK);`, back faces will not be - * drawn ("culled") - */ - void cullFaces(const TCullFace& cf) { m_cullface = cf; } - TCullFace cullFaces() const { return m_cullface; } - /** @name Raw access to triangle shader buffer data * @{ */ const auto& shaderTrianglesBuffer() const { return m_triangles; } @@ -75,15 +64,9 @@ class CRenderizableShaderTriangles : public virtual CRenderizable /** Returns the bounding box of m_triangles, or (0,0,0)-(0,0,0) if empty. */ const mrpt::math::TBoundingBoxf trianglesBoundingBox() const; - void params_serialize(mrpt::serialization::CArchive& out) const; - void params_deserialize(mrpt::serialization::CArchive& in); - private: mutable Buffer m_trianglesBuffer; mutable VertexArrayObject m_vao; - - bool m_enableLight = true; - TCullFace m_cullface = TCullFace::NONE; }; } // namespace mrpt::opengl diff --git a/libs/opengl/include/mrpt/opengl/CRenderizableShaderWireFrame.h b/libs/opengl/include/mrpt/opengl/CRenderizableShaderWireFrame.h index ffc8bf595b..3ddf79d0d6 100644 --- a/libs/opengl/include/mrpt/opengl/CRenderizableShaderWireFrame.h +++ b/libs/opengl/include/mrpt/opengl/CRenderizableShaderWireFrame.h @@ -39,19 +39,6 @@ class CRenderizableShaderWireFrame : public virtual CRenderizable * to be drawn in "m_*_buffer" fields. */ virtual void onUpdateBuffers_Wireframe() = 0; - void setLineWidth(float w) - { - m_lineWidth = w; - CRenderizable::notifyChange(); - } - float getLineWidth() const { return m_lineWidth; } - void enableAntiAliasing(bool enable = true) - { - m_antiAliasing = enable; - CRenderizable::notifyChange(); - } - bool isAntiAliasingEnabled() const { return m_antiAliasing; } - // See base docs void freeOpenGLResources() override { @@ -73,9 +60,6 @@ class CRenderizableShaderWireFrame : public virtual CRenderizable mutable std::vector m_color_buffer_data; mutable mrpt::containers::NonCopiableData m_wireframeMtx; - float m_lineWidth = 1.0f; - bool m_antiAliasing = false; - /** Returns the bounding box of m_vertex_buffer_data, or (0,0,0)-(0,0,0) if * empty. */ const mrpt::math::TBoundingBox wireframeVerticesBoundingBox() const; diff --git a/libs/opengl/src/CPointCloud.cpp b/libs/opengl/src/CPointCloud.cpp index ed6ed2e358..44d8d4bd99 100644 --- a/libs/opengl/src/CPointCloud.cpp +++ b/libs/opengl/src/CPointCloud.cpp @@ -202,7 +202,6 @@ void CPointCloud::serializeTo(mrpt::serialization::CSchemeArchiveBase& out) cons out["colorFromDepth_max"]["R"] = m_colorFromDepth_max.R; out["colorFromDepth_max"]["G"] = m_colorFromDepth_max.G; out["colorFromDepth_max"]["B"] = m_colorFromDepth_max.B; - out["pointSmooth"] = m_pointSmooth; } void CPointCloud::serializeFrom(mrpt::serialization::CSchemeArchiveBase& in) { @@ -232,7 +231,6 @@ void CPointCloud::serializeFrom(mrpt::serialization::CSchemeArchiveBase& in) m_colorFromDepth_max.R = static_cast(in["colorFromDepth_max"]["R"]); m_colorFromDepth_max.G = static_cast(in["colorFromDepth_max"]["G"]); m_colorFromDepth_max.B = static_cast(in["colorFromDepth_max"]["B"]); - m_pointSmooth = static_cast(in["pointSmooth"]); } break; default: diff --git a/libs/opengl/src/CRenderizableShaderPoints.cpp b/libs/opengl/src/CRenderizableShaderPoints.cpp index 89f14604ef..5731fb650d 100644 --- a/libs/opengl/src/CRenderizableShaderPoints.cpp +++ b/libs/opengl/src/CRenderizableShaderPoints.cpp @@ -132,18 +132,3 @@ void CRenderizableShaderPoints::params_deserialize(mrpt::serialization::CArchive MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); }; } - -const mrpt::math::TBoundingBoxf CRenderizableShaderPoints::verticesBoundingBox() const -{ - std::shared_lock wfReadLock(CRenderizableShaderPoints::m_pointsMtx.data); - - mrpt::math::TBoundingBoxf bb; - - if (m_vertex_buffer_data.empty()) return bb; - - bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); - - for (const auto& p : m_vertex_buffer_data) bb.updateWithPoint(p); - - return bb; -} diff --git a/libs/opengl/src/CRenderizableShaderTexturedTriangles.cpp b/libs/opengl/src/CRenderizableShaderTexturedTriangles.cpp index f8096e4f2f..fe10cde6cd 100644 --- a/libs/opengl/src/CRenderizableShaderTexturedTriangles.cpp +++ b/libs/opengl/src/CRenderizableShaderTexturedTriangles.cpp @@ -185,74 +185,6 @@ void CRenderizableShaderTexturedTriangles::render(const RenderContext& rc) const #endif } -void CRenderizableShaderTexturedTriangles::assignImage(const CImage& img, const CImage& imgAlpha) -{ - MRPT_START - - CRenderizable::notifyChange(); - - m_glTexture.unloadTexture(); - - // Make a copy: - m_textureImage = img; - m_textureImageAlpha = imgAlpha; - m_textureImageAssigned = true; - - m_enableTransparency = true; - - MRPT_END -} - -void CRenderizableShaderTexturedTriangles::assignImage(const CImage& img) -{ - MRPT_START - - CRenderizable::notifyChange(); - - m_glTexture.unloadTexture(); - - // Make a shallow copy: - m_textureImage = img; - m_textureImageAssigned = true; - - m_enableTransparency = false; - - MRPT_END -} - -void CRenderizableShaderTexturedTriangles::assignImage(CImage&& img, CImage&& imgAlpha) -{ - MRPT_START - - CRenderizable::notifyChange(); - - m_glTexture.unloadTexture(); - - m_textureImage = std::move(img); - m_textureImageAlpha = std::move(imgAlpha); - m_textureImageAssigned = true; - - m_enableTransparency = true; - - MRPT_END -} - -void CRenderizableShaderTexturedTriangles::assignImage(CImage&& img) -{ - MRPT_START - - CRenderizable::notifyChange(); - - m_glTexture.unloadTexture(); - - m_textureImage = std::move(img); - m_textureImageAssigned = true; - - m_enableTransparency = false; - - MRPT_END -} - void CRenderizableShaderTexturedTriangles::initializeTextures() const { #if MRPT_HAS_OPENGL_GLUT || MRPT_HAS_EGL @@ -298,83 +230,3 @@ CRenderizableShaderTexturedTriangles::~CRenderizableShaderTexturedTriangles() << mrpt::exception_to_str(e); } } - -void CRenderizableShaderTexturedTriangles::writeToStreamTexturedObject( - mrpt::serialization::CArchive& out) const -{ - uint8_t ver = 3; - - out << ver; - out << m_enableTransparency << m_textureInterpolate << m_textureUseMipMaps; - out << m_textureImage; - if (m_enableTransparency) out << m_textureImageAlpha; - out << m_textureImageAssigned; - out << m_enableLight << static_cast(m_cullface); // v2 -} - -void CRenderizableShaderTexturedTriangles::readFromStreamTexturedObject( - mrpt::serialization::CArchive& in) -{ - uint8_t version; - in >> version; - - switch (version) - { - case 0: - case 1: - case 2: - case 3: - { - in >> m_enableTransparency >> m_textureInterpolate; - if (version >= 3) - { - in >> m_textureUseMipMaps; - } - else - { - m_textureUseMipMaps = true; - } - in >> m_textureImage; - if (m_enableTransparency) - { - in >> m_textureImageAlpha; - assignImage(m_textureImage, m_textureImageAlpha); - } - else - { - assignImage(m_textureImage); - } - if (version >= 1) - in >> m_textureImageAssigned; - else - m_textureImageAssigned = true; - - if (version >= 2) - { - in >> m_enableLight; - m_cullface = static_cast(in.ReadAs()); - } - } - break; - default: - MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); - }; - - CRenderizable::notifyChange(); -} - -const mrpt::math::TBoundingBoxf CRenderizableShaderTexturedTriangles::trianglesBoundingBox() const -{ - mrpt::math::TBoundingBoxf bb; - - std::shared_lock readLock(m_trianglesMtx.data); - - if (m_triangles.empty()) return bb; - - bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); - - for (const auto& t : m_triangles) - for (int i = 0; i < 3; i++) bb.updateWithPoint(t.vertices[i].xyzrgba.pt); - - return bb; -} diff --git a/libs/opengl/src/CRenderizableShaderTriangles.cpp b/libs/opengl/src/CRenderizableShaderTriangles.cpp index ad3db455c0..688273dc26 100644 --- a/libs/opengl/src/CRenderizableShaderTriangles.cpp +++ b/libs/opengl/src/CRenderizableShaderTriangles.cpp @@ -185,23 +185,3 @@ const math::TBoundingBoxf CRenderizableShaderTriangles::trianglesBoundingBox() c return bb; } - -void CRenderizableShaderTriangles::params_serialize(mrpt::serialization::CArchive& out) const -{ - out.WriteAs(0); // serialization version - out << m_enableLight << static_cast(m_cullface); -} -void CRenderizableShaderTriangles::params_deserialize(mrpt::serialization::CArchive& in) -{ - const auto version = in.ReadAs(); - - switch (version) - { - case 0: - in >> m_enableLight; - m_cullface = static_cast(in.ReadAs()); - break; - default: - MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); - }; -} diff --git a/libs/viz/CMakeLists.txt b/libs/viz/CMakeLists.txt new file mode 100644 index 0000000000..06738a3372 --- /dev/null +++ b/libs/viz/CMakeLists.txt @@ -0,0 +1,19 @@ +# Lists of directories with source files: +# See "DeclareMRPTLib.cmake" for explanations +# ------------------------------------------------- + +#--------------------------------------------- +# Macro declared in "DeclareMRPTLib.cmake": +#--------------------------------------------- +define_mrpt_lib( + # Lib name + viz + # Dependencies: + mrpt-poses + mrpt-img + ) + +if(NOT BUILD_mrpt-viz) + return() +endif() + diff --git a/libs/viz/include/mrpt/viz/CArrow.h b/libs/viz/include/mrpt/viz/CArrow.h new file mode 100644 index 0000000000..160c996e49 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CArrow.h @@ -0,0 +1,127 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +namespace mrpt::viz +{ +/** A 3D arrow + * + * ![mrpt::viz::CArrow](preview_CArrow.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CArrow : virtual public CVisualObject, public VisualObjectParams_Triangles +{ + DEFINE_SERIALIZABLE(CArrow, mrpt::viz) + DEFINE_SCHEMA_SERIALIZABLE() + + public: + void setArrowEnds(float x0, float y0, float z0, float x1, float y1, float z1) + { + m_x0 = x0; + m_y0 = y0; + m_z0 = z0; + m_x1 = x1; + m_y1 = y1; + m_z1 = z1; + CVisualObject::notifyChange(); + } + template + void setArrowEnds(const Vector3Like& start, const Vector3Like& end) + { + m_x0 = start[0]; + m_y0 = start[1]; + m_z0 = start[2]; + m_x1 = end[0]; + m_y1 = end[1]; + m_z1 = end[2]; + CVisualObject::notifyChange(); + } + void setHeadRatio(float rat) + { + m_headRatio = rat; + CVisualObject::notifyChange(); + } + void setSmallRadius(float rat) + { + m_smallRadius = rat; + CVisualObject::notifyChange(); + } + void setLargeRadius(float rat) + { + m_largeRadius = rat; + CVisualObject::notifyChange(); + } + /** Number of radial divisions */ + void setSlicesCount(uint32_t slices) + { + m_slices = slices; + CVisualObject::notifyChange(); + } + + /** Number of radial divisions */ + uint32_t getSlicesCount() const { return m_slices; } + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Constructor */ + CArrow( + float x0 = 0, + float y0 = 0, + float z0 = 0, + float x1 = 1, + float y1 = 1, + float z1 = 1, + float headRatio = 0.2f, + float smallRadius = 0.05f, + float largeRadius = 0.2f) : + m_x0(x0), + m_y0(y0), + m_z0(z0), + m_x1(x1), + m_y1(y1), + m_z1(z1), + m_headRatio(headRatio), + m_smallRadius(smallRadius), + m_largeRadius(largeRadius) + { + } + + CArrow( + const mrpt::math::TPoint3Df& from, + const mrpt::math::TPoint3Df& to, + float headRatio = 0.2f, + float smallRadius = 0.05f, + float largeRadius = 0.2f) : + m_x0(from.x), + m_y0(from.y), + m_z0(from.z), + m_x1(to.x), + m_y1(to.y), + m_z1(to.z), + m_headRatio(headRatio), + m_smallRadius(smallRadius), + m_largeRadius(largeRadius) + { + } + ~CArrow() override = default; + + protected: + mutable float m_x0, m_y0, m_z0; + mutable float m_x1, m_y1, m_z1; + float m_headRatio; + float m_smallRadius, m_largeRadius; + /** Number of radial divisions. */ + uint32_t m_slices = 10; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CAssimpModel.h b/libs/viz/include/mrpt/viz/CAssimpModel.h new file mode 100644 index 0000000000..0bb10275ae --- /dev/null +++ b/libs/viz/include/mrpt/viz/CAssimpModel.h @@ -0,0 +1,106 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include + +#include +#include + +namespace mrpt::viz +{ +/** This class can load & render 3D models in a number of different formats + * (requires the library assimp). + * - All supported formats: + * http://assimp.sourceforge.net/main_features_formats.html + * - Most common ones: AutoCAD DXF ( .dxf ), Collada ( .dae ), Blender 3D ( + * .blend ), 3ds Max 3DS ( .3ds ), 3ds Max ASE ( .ase ), Quake I ( .mdl ), Quake + * II ( .md2 ), Quake III Mesh ( .md3 ), etc. + * + * Models are loaded via CAssimpModel::loadScene() + * + * ![mrpt::viz::CAssimpModel](preview_CAssimpModel.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CAssimpModel : public CVisualObject +{ + DEFINE_SERIALIZABLE(CAssimpModel, mrpt::viz) + + public: + CAssimpModel(); + virtual ~CAssimpModel() override; + + /** Import flags for loadScene + * \note Not defined as ``enum class`` to allow C++-valid or-wise combinations + */ + struct LoadFlags + { + enum flags_t : uint32_t + { + /** See: aiProcessPreset_TargetRealtime_Fast */ + RealTimeFast = 0x0001, + + /** See: aiProcessPreset_TargetRealtime_Quality */ + RealTimeQuality = 0x0002, + + /** See: aiProcessPreset_TargetRealtime_MaxQuality */ + RealTimeMaxQuality = 0x0004, + + /** See: aiProcess_FlipUVs */ + FlipUVs = 0x0010, + + /** MRPT-specific: ignore materials and replace by the base class + CRenderizable uniform color that was defined before calling + loadScene(). \note (New in MRPT 2.5.0) */ + IgnoreMaterialColor = 0x0100, + + /** Displays messages on loaded textures, etc. */ + Verbose = 0x1000 + }; + }; + + using filepath_t = std::string; + + /** Loads a scene from a file in any supported file. + * \exception std::runtime_error On any error during loading or importing + * the file. + */ + void loadScene( + const std::string& file_name, + const int flags = LoadFlags::RealTimeMaxQuality | LoadFlags::FlipUVs | LoadFlags::Verbose); + + /** Empty the object */ + void clear(); + + /* Simulation of ray-trace. */ + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Enable (or disable if set to .0f) a feature in which textured triangles + * are split into different renderizable smaller objects. + * This is required only for semitransparent objects with overlaping regions. + */ + void split_triangles_rendering_bbox(const float bbox_size); + + [[nodiscard]] float split_triangles_rendering_bbox() const + { + return m_split_triangles_rendering_bbox; + } + + private: + filepath_t m_modelPath; + uint32_t m_modelLoadFlags = 0; + float m_split_triangles_rendering_bbox = .0f; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CAxis.h b/libs/viz/include/mrpt/viz/CAxis.h new file mode 100644 index 0000000000..ac093e7315 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CAxis.h @@ -0,0 +1,75 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +#include + +namespace mrpt::viz +{ +/** Draw a 3D world axis, with coordinate marks at some regular interval + * \sa opengl::Scene + * + * ![mrpt::viz::CAxis](preview_CAxis.png) + * + * \ingroup mrpt_viz_grp + */ +class CAxis : virtual public CVisualObject, public VisualObjectParams_Lines +{ + DEFINE_SERIALIZABLE(CAxis, mrpt::viz) + public: + /** Constructor */ + CAxis( + float xmin = -1.0f, + float ymin = -1.0f, + float zmin = -1.0f, + float xmax = 1.0f, + float ymax = 1.0f, + float zmax = 1.0f, + float frecuency = 1.f, + float lineWidth = 3.0f, + bool marks = true); + + virtual ~CAxis() override = default; + + void setAxisLimits(float xmin, float ymin, float zmin, float xmax, float ymax, float zmax); + /** Changes the frequency of the "ticks" */ + void setFrequency(float f); + float getFrequency() const; + /** Changes the size of text labels (default:0.25) */ + void setTextScale(float f); + float getTextScale() const; + /** axis: {0,1,2}=>{X,Y,Z} */ + void setTextLabelOrientation(int axis, float yaw_deg, float pitch_deg, float roll_deg); + /** axis: {0,1,2}=>{X,Y,Z} */ + void getTextLabelOrientation(int axis, float& yaw_deg, float& pitch_deg, float& roll_deg) const; + + void enableTickMarks(bool v = true); + void enableTickMarks(bool show_x, bool show_y, bool show_z); + /** As a ratio of "marks frequency" (default: 0.05) */ + void setTickMarksLength(float len); + float getTickMarksLength(float len) { return m_markLen; } + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + protected: + float m_xmin, m_ymin, m_zmin; + float m_xmax, m_ymax, m_zmax; + float m_frequency; + /** draw marks for X,Y,Z */ + std::array m_marks = {false, false, false}; + float m_textScale{0.10f}; + float m_textRot[3][3]; // {x,y,z},{yaw,pitch,roll} + float m_markLen{0.07f}; + + mrpt::containers::PerThreadDataHolder m_gl_labels; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CBox.h b/libs/viz/include/mrpt/viz/CBox.h new file mode 100644 index 0000000000..6e1fb09d9b --- /dev/null +++ b/libs/viz/include/mrpt/viz/CBox.h @@ -0,0 +1,101 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include + +namespace mrpt::viz +{ +/** A solid or wireframe box in 3D, defined by 6 rectangular faces parallel to + *the planes X, Y and Z (note that the object can be translated and rotated + *afterwards as any other CRenderizable object using the "object pose" in the + *base class). + * Three drawing modes are possible: + * - Wireframe: setWireframe(true). Used color is the CRenderizable color + * - Solid box: setWireframe(false). Used color is the CRenderizable color + * - Solid box with border: setWireframe(false) + enableBoxBorder(true). Solid + *color is the CRenderizable color, border line can be set with + *setBoxBorderColor(). + * + * ![mrpt::viz::CBox](preview_CBox.png) + * + * \sa mrpt::viz::Scene, mrpt::viz::CRenderizable + * \ingroup mrpt_viz_grp + */ +class CBox : + virtual public CVisualObject, + virtual public VisualObjectParams_Lines, + virtual public VisualObjectParams_Triangles +{ + DEFINE_SERIALIZABLE(CBox, mrpt::viz) + + public: + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** + * Ray tracing. + * \sa mrpt::viz::CRenderizable + */ + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + + void setWireframe(bool is_wireframe = true) + { + m_wireframe = is_wireframe; + CVisualObject::notifyChange(); + } + bool isWireframe() const { return m_wireframe; } + void enableBoxBorder(bool drawBorder = true) + { + m_draw_border = drawBorder; + CVisualObject::notifyChange(); + } + bool isBoxBorderEnabled() const { return m_draw_border; } + void setBoxBorderColor(const mrpt::img::TColor& c) + { + m_solidborder_color = c; + CVisualObject::notifyChange(); + } + mrpt::img::TColor getBoxBorderColor() const { return m_solidborder_color; } + + /** Set the position and size of the box, from two corners in 3D */ + void setBoxCorners(const mrpt::math::TPoint3D& corner1, const mrpt::math::TPoint3D& corner2); + void getBoxCorners(mrpt::math::TPoint3D& corner1, mrpt::math::TPoint3D& corner2) const + { + corner1 = m_corner_min; + corner2 = m_corner_max; + } + + /** Basic empty constructor. Set all parameters to default. */ + CBox() = default; + + /** Constructor with all the parameters */ + CBox( + const mrpt::math::TPoint3D& corner1, + const mrpt::math::TPoint3D& corner2, + bool is_wireframe = false, + float lineWidth = 1.0); + + /** Destructor */ + ~CBox() override = default; + + protected: + /** Corners coordinates */ + mrpt::math::TPoint3D m_corner_min = {-1, -1, -1}, m_corner_max = {1, 1, 1}; + /** true: wireframe, false (default): solid */ + bool m_wireframe{false}; + + /** Draw line borders to solid box with the given linewidth (default: true) + */ + bool m_draw_border{true}; + + /** Color of the solid box borders. */ + mrpt::img::TColor m_solidborder_color = {0, 0, 0}; +}; +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CCamera.h b/libs/viz/include/mrpt/viz/CCamera.h new file mode 100644 index 0000000000..2e8d934648 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CCamera.h @@ -0,0 +1,185 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include +#include +#include + +#include + +namespace mrpt::viz +{ +/** Defines the intrinsic and extrinsic camera coordinates from which to render + * a 3D scene. + * + * By default, each viewport has its own camera, accessible via + * Viewport::getCamera(), but if a CCamera object is added as an object + * to be rendered, it will override the internal viewport camera. + * + * Available projection models: + * - Projective model, parameterized via setProjectiveFOVdeg() (vertical field + * of view, in degrees) + * - Projective model, by means of a computer vision pinhole intrinsic + * parameter set: see setProjectiveFromPinhole() + * - Orthogonal projection model: use setProjectiveModel(false), or + * setOrthogonal(), or + * - No projection mode: use `setNoProjection()`. Viewport coordinates are + * fixed to bottomLeft=(-1,-1) to rightTop=(+1,+1). + * + * Defining the position and orientation of a camera is possible by: + * - Using an "orbit" model: defined by a "pointing to" point, a distance to + * object, and azimuth + elevation angles; or + * - Directly giving the SE(3) camera pose, setting the set6DOFMode() to `true` + * and storing the desired pose with CVisualObject::setPose(). Pose axis + * convention is +Z pointing forwards, +X right, +Y down. + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CCamera : public CVisualObject +{ + DEFINE_SERIALIZABLE(CCamera, mrpt::viz) + public: + CCamera() = default; + ~CCamera() override = default; + + void toYAMLMap(mrpt::containers::yaml& propertiesMap) const override; + + /** @name Projection model (camera intrinsic parameters) + * @{ */ + + /** Enable/Disable projective mode (vs. orthogonal). */ + void setProjectiveModel(bool v = true) + { + m_projectiveModel = v; + m_useNoProjection = false; + } + + /** Enable/Disable orthogonal mode (vs. projective)*/ + void setOrthogonal(bool v = true) + { + m_projectiveModel = !v; + m_useNoProjection = false; + } + + void setProjectiveFromPinhole(const mrpt::img::TCamera& camIntrinsics) + { + m_projectiveModel = true; + m_useNoProjection = false; + m_pinholeModel = camIntrinsics; + } + + /** Disable all coordinate transformations and allow direct use of pixel + * coordinates, that is, the projection matrix is the identity. + * + * In this mode, (-1,-1) is the bottom-left corner and (+1,+1) the top-right + * corner, per OpenGL defaults. This mode can be disabled calling + * setProjectiveModel() or setOrthogonal() + */ + void setNoProjection() { m_useNoProjection = true; } + + /** Vertical field-of-View in degs, only when projectiveModel=true + * (default=30 deg). + */ + void setProjectiveFOVdeg(float ang) { m_projectiveFOVdeg = ang; } + + /** Field-of-View in degs, only when projectiveModel=true (default=30 deg). + */ + [[nodiscard]] float getProjectiveFOVdeg() const { return m_projectiveFOVdeg; } + + [[nodiscard]] bool hasPinholeModel() const { return m_pinholeModel.has_value(); } + + const std::optional& getPinholeModel() const { return m_pinholeModel; } + + [[nodiscard]] bool isProjective() const { return m_projectiveModel; } + [[nodiscard]] bool isOrthogonal() const { return !m_projectiveModel; } + [[nodiscard]] bool isNoProjection() const { return m_useNoProjection; } + + /** @} */ + + /** @name Defines camera pose (camera extrinsic parameters) + * @{ */ + + void setPointingAt(float x, float y, float z) + { + m_pointingX = x; + m_pointingY = y; + m_pointingZ = z; + } + + template + void setPointingAt(const POSEORPOINT& p) + { + m_pointingX = p.x(); + m_pointingY = p.y(); + m_pointingZ = p.is3DPoseOrPoint() ? p.m_coords[2] : 0; + } + void setPointingAt(const mrpt::math::TPoint3D& p) { setPointingAt(d2f(p.x), d2f(p.y), d2f(p.z)); } + + [[nodiscard]] float getPointingAtX() const { return m_pointingX; } + [[nodiscard]] float getPointingAtY() const { return m_pointingY; } + [[nodiscard]] float getPointingAtZ() const { return m_pointingZ; } + + [[nodiscard]] mrpt::math::TPoint3Df getPointingAt() const + { + return {m_pointingX, m_pointingY, m_pointingZ}; + } + + [[nodiscard]] float getZoomDistance() const { return m_eyeDistance; } + [[nodiscard]] float getAzimuthDegrees() const { return m_azimuthDeg; } + [[nodiscard]] float getElevationDegrees() const { return m_elevationDeg; } + [[nodiscard]] float getRollDegrees() const { return m_eyeRollDeg; } + + void setZoomDistance(float z) { m_eyeDistance = z; } + void setAzimuthDegrees(float ang) { m_azimuthDeg = ang; } + void setElevationDegrees(float ang) { m_elevationDeg = ang; } + void setRollDegrees(float ang) { m_eyeRollDeg = ang; } + + /** Set 6DOFMode, if enabled camera is set according to its pose, set via + *CVisualObject::setPose(). (default=false). + * Conventionally, eye is set looking towards +Z axis, "down" is the +Y + * axis, right is "+X" axis. In this mode azimuth/elevation are ignored. + */ + void set6DOFMode(bool v) { m_6DOFMode = v; } + + [[nodiscard]] bool is6DOFMode() const { return m_6DOFMode; } + + /** @} */ + + /** In this class, returns a fixed box (max,max,max), (-max,-max,-max). */ + [[nodiscard]] mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + protected: + float m_pointingX{0}, m_pointingY{0}, m_pointingZ{0}; + float m_eyeDistance{10}; + float m_azimuthDeg{45}, m_elevationDeg{45}; + float m_eyeRollDeg{0}; + + /** If set to true (default), camera model is projective, otherwise, it's + * orthogonal. */ + bool m_projectiveModel{true}; + + /// See setNoProjection() + bool m_useNoProjection = false; + + /** If defined, it overrides m_projectiveFOVdeg. */ + std::optional m_pinholeModel; + + /** Field-of-View in degs, only when projectiveModel=true and there is no + * pinhole model assigned. (default=30 deg). + */ + float m_projectiveFOVdeg{30}; + + /** If set to true, camera pose is used when rendering the viewport */ + bool m_6DOFMode{false}; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CColorBar.h b/libs/viz/include/mrpt/viz/CColorBar.h new file mode 100644 index 0000000000..0515f7a19b --- /dev/null +++ b/libs/viz/include/mrpt/viz/CColorBar.h @@ -0,0 +1,60 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include + +namespace mrpt::viz +{ +/** A colorbar indicator. This class renders a colorbar as a 3D object, in the + * XY plane. + * For an overlay indicator that can be easily added to any display, see + * Scene::addColorBar() + * + * ![mrpt::viz::CColorBar](preview_CColorBar.png) + * + * \sa mrpt::viz::Scene, mrpt::viz::CRenderizable, Scene::addColorBar() + * \ingroup mrpt_viz_grp + */ +class CColorBar : virtual public CVisualObject, public VisualObjectParams_Triangles +{ + DEFINE_SERIALIZABLE(CColorBar, mrpt::viz) + + public: + CColorBar(/** The colormap to represent. */ + const mrpt::img::TColormap colormap = mrpt::img::cmGRAYSCALE, + /** size of the color bar */ + double width = 0.2, + double height = 1.0, + /** limits for [0,1] colormap indices */ + float min_col = .0, + float max_col = 1.0, + /** limits for values associated to extreme colors */ + float min_value = .0, + float max_value = 1.0, + /** sprintf-like format string for values */ + const std::string& label_format = std::string("%7.02f"), + /** Label text font size */ + float label_font_size = .05f); + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + void setColormap(const mrpt::img::TColormap colormap); + void setColorAndValueLimits(float col_min, float col_max, float value_min, float value_max); + + protected: + mrpt::img::TColormap m_colormap; + double m_width, m_height; + std::string m_label_format; + float m_min_col, m_max_col, m_min_value, m_max_value; + float m_label_font_size; +}; +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CCylinder.h b/libs/viz/include/mrpt/viz/CCylinder.h new file mode 100644 index 0000000000..e782ff1ebe --- /dev/null +++ b/libs/viz/include/mrpt/viz/CCylinder.h @@ -0,0 +1,170 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +namespace mrpt::viz +{ +class CCylinder; +/** A cylinder or cone whose base lies in the XY plane. + * + * ![mrpt::viz::CCylinder](preview_CCylinder.png) + * + * \sa opengl::Scene,opengl::CDisk + * \ingroup mrpt_viz_grp + */ +class CCylinder : virtual public CVisualObject, public VisualObjectParams_Triangles +{ + DEFINE_SERIALIZABLE(CCylinder, mrpt::viz) + DEFINE_SCHEMA_SERIALIZABLE() + public: + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + + /** + * Configuration of the cylinder's bases display. + */ + void setHasBases(bool top = true, bool bottom = true) + { + m_hasTopBase = top; + m_hasBottomBase = bottom; + CVisualObject::notifyChange(); + } + /** + * Check whether top base is displayed. + * \sa hasBottomBase + */ + [[nodiscard]] bool hasTopBase() const { return m_hasTopBase; } + + /** + * Check whether bottom base is displayed. + * \sa hasTopBase + */ + [[nodiscard]] bool hasBottomBase() const { return m_hasBottomBase; } + + /** + * Sets both radii to a single value, thus configuring the object as a + * cylinder. + * \sa setRadii + */ + void setRadius(float radius) + { + m_baseRadius = m_topRadius = radius; + CVisualObject::notifyChange(); + } + + /** + * Sets both radii independently. + * \sa setRadius + */ + void setRadii(float bottom, float top) + { + m_baseRadius = bottom; + m_topRadius = top; + CVisualObject::notifyChange(); + } + + /** + * Chenges cylinder's height. + */ + void setHeight(float height) + { + m_height = height; + CVisualObject::notifyChange(); + } + + /** Gets the bottom radius. */ + [[nodiscard]] float getBottomRadius() const { return m_baseRadius; } + + /** Gets the top radius. */ + [[nodiscard]] float getTopRadius() const { return m_topRadius; } + + /**Gets the cylinder's height.*/ + [[nodiscard]] float getHeight() const { return m_height; } + + /** Number of radial divisions */ + void setSlicesCount(uint32_t slices) + { + m_slices = slices; + CVisualObject::notifyChange(); + } + + /** Number of radial divisions */ + [[nodiscard]] uint32_t getSlicesCount() const { return m_slices; } + + /** Evaluates the bounding box of this object (including possible children) + * in the coordinate frame of the object parent. */ + [[nodiscard]] mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + CCylinder() = default; + + /** + * Complete constructor. Allows the configuration of every parameter. + */ + /** Constructor with two radii. Allows the construction of any cylinder. */ + CCylinder( + const float baseRadius, + const float topRadius, + const float height = 1, + const int slices = 10) : + m_baseRadius(baseRadius), + m_topRadius(topRadius), + m_height(height), + m_slices(slices), + m_hasTopBase(true), + m_hasBottomBase(true) + { + } + + /** Destructor */ + ~CCylinder() override = default; + + protected: + /** + * Cylinder's radii. If m_baseRadius==m_topRadius, then the object is an + * actual cylinder. If both differ, it's a truncated cone. If one of the + * radii is zero, the object is a cone. + */ + float m_baseRadius{1}, m_topRadius{1}; + /** + * Cylinder's height + */ + float m_height{1}; + + /** Number of radial divisions. */ + uint32_t m_slices{10}; + + /** + * Boolean parameters about including the bases in the object. If both + * m_hasTopBase and m_hasBottomBase are set to false, only the lateral area + * is displayed. + */ + bool m_hasTopBase{true}, m_hasBottomBase{true}; + + private: + /** + * Gets the radius of the circunference located at certain height, + * returning false if the cylinder doesn't get that high. + */ + [[nodiscard]] bool getRadius(float Z, float& r) const + { + if (!reachesHeight(Z)) return false; + r = (Z / m_height) * (m_topRadius - m_baseRadius) + m_baseRadius; + return true; + } + /** + * Checks whether the cylinder exists at some height. + */ + [[nodiscard]] bool reachesHeight(float Z) const + { + return (m_height < 0) ? (Z >= m_height && Z <= 0) : (Z <= m_height && Z >= 0); + } + [[nodiscard]] bool reachesHeight(double Z) const { return reachesHeight(d2f(Z)); } +}; +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CDisk.h b/libs/viz/include/mrpt/viz/CDisk.h new file mode 100644 index 0000000000..4cdf6eceb4 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CDisk.h @@ -0,0 +1,70 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include + +namespace mrpt::viz +{ +/** A planar disk in the XY plane. + * + * ![mrpt::viz::CDisk](preview_CDisk.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CDisk : virtual public CVisualObject, public VisualObjectParams_Triangles +{ + DEFINE_SERIALIZABLE(CDisk, mrpt::viz) + + public: + void setDiskRadius(float outRadius, float inRadius = 0) + { + m_radiusIn = inRadius; + m_radiusOut = outRadius; + CVisualObject::notifyChange(); + } + + [[nodiscard]] float getInRadius() const { return m_radiusIn; } + [[nodiscard]] float getOutRadius() const { return m_radiusOut; } + + /** Default=50 */ + void setSlicesCount(uint32_t N) + { + m_nSlices = N; + CVisualObject::notifyChange(); + } + + /** Evaluates the bounding box of this object (including possible children) + * in the coordinate frame of the object parent. */ + [[nodiscard]] mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Ray tracing + */ + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + + /** Constructor + */ + CDisk() = default; + CDisk(float rOut, float rIn, uint32_t slices = 50) : + m_radiusIn(rIn), m_radiusOut(rOut), m_nSlices(slices) + { + } + + /** Private, virtual destructor: only can be deleted from smart pointers */ + ~CDisk() override = default; + + protected: + float m_radiusIn = 0, m_radiusOut = 1; + uint32_t m_nSlices = 50; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CEllipsoid2D.h b/libs/viz/include/mrpt/viz/CEllipsoid2D.h new file mode 100644 index 0000000000..333462c905 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CEllipsoid2D.h @@ -0,0 +1,60 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include + +namespace mrpt::viz +{ +/** A 2D ellipse on the XY plane, centered at the origin of this object pose. + * + * The color is determined by the RGBA fields in the class "CRenderizable". + * Note that a transparent ellipse can be drawn for "0 +{ + using BASE = CGeneralizedEllipsoidTemplate<2>; + + DEFINE_SERIALIZABLE(CEllipsoid2D, mrpt::viz) + + public: + CEllipsoid2D() = default; + virtual ~CEllipsoid2D() override = default; + + /** The number of segments of a 2D ellipse (default=20) */ + void set2DsegmentsCount(unsigned int N) { BASE::setNumberOfSegments(N); } + + /** Ray tracing */ + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + + protected: + /** To be implemented by derived classes: maps, using some arbitrary space + * transformation, a list of points + * defining an ellipsoid in parameter space into their corresponding + * points in 2D/3D space. + */ + void transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const override; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CEllipsoid3D.h b/libs/viz/include/mrpt/viz/CEllipsoid3D.h new file mode 100644 index 0000000000..3aaa74daa2 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CEllipsoid3D.h @@ -0,0 +1,59 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include + +namespace mrpt::viz +{ +/** A 3D ellipsoid, centered at zero with respect to this object pose. + * The color is determined by the RGBA fields in the class "CRenderizable". + * Note that a transparent ellipsoid can be drawn for "0 +{ + using BASE = CGeneralizedEllipsoidTemplate<3>; + + DEFINE_SERIALIZABLE(CEllipsoid3D, mrpt::viz) + + public: + CEllipsoid3D() = default; + virtual ~CEllipsoid3D() override = default; + + /** The number of segments of a 3D ellipse (in both "axes") (default=20) */ + void set3DsegmentsCount(unsigned int N) { BASE::setNumberOfSegments(N); } + + /** Ray tracing */ + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + + protected: + /** To be implemented by derived classes: maps, using some arbitrary space + * transformation, a list of points + * defining an ellipsoid in parameter space into their corresponding + * points in 2D/3D space. + */ + void transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const override; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CEllipsoidInverseDepth2D.h b/libs/viz/include/mrpt/viz/CEllipsoidInverseDepth2D.h new file mode 100644 index 0000000000..d6599bf759 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CEllipsoidInverseDepth2D.h @@ -0,0 +1,65 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +namespace mrpt::viz +{ +/** An especial "ellipsoid" in 3D computed as the uncertainty iso-surfaces of a + * (inv_range,yaw) variable. + * The parameter space of this ellipsoid comprises these variables (in this + * order): + * - inv_range: The inverse distance from the sensor to the feature. + * - yaw: Angle for the rotation around +Z ("azimuth"). + * + * This parameterization is a 2D version of that presented in the paper: + * - Civera, J. and Davison, A.J. and Montiel, J., "Inverse depth + * parametrization for monocular SLAM", T-RO, 2008. + * + * This class expects you to provide a mean vector of length 4 and a 4x4 + * covariance matrix, set with \a setCovMatrixAndMean(). + * + * Please read the documentation of + * CGeneralizedEllipsoidTemplate::setQuantiles() for learning + * the mathematical details about setting the desired confidence interval. + * + * ![mrpt::viz::CEllipsoidInverseDepth2D](preview_CEllipsoidInverseDepth2D.png) + * + * \ingroup mrpt_viz_grp + */ +class CEllipsoidInverseDepth2D : public CGeneralizedEllipsoidTemplate<2> +{ + using BASE = CGeneralizedEllipsoidTemplate<2>; + DEFINE_SERIALIZABLE(CEllipsoidInverseDepth2D, mrpt::viz) + + public: + CEllipsoidInverseDepth2D() = default; + virtual ~CEllipsoidInverseDepth2D() override = default; + + /** The maximum range to be used as a correction when a point of the + * ellipsoid falls in the negative ranges (default: 1e6) */ + void setUnderflowMaxRange(const double maxRange) { m_underflowMaxRange = maxRange; } + double getUnderflowMaxRange() const { return m_underflowMaxRange; } + + protected: + /** To be implemented by derived classes: maps, using some arbitrary space + * transformation, a list of points + * defining an ellipsoid in parameter space into their corresponding + * points in 2D/3D space. + */ + void transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const override; + + private: + double m_underflowMaxRange{1e6}; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CEllipsoidInverseDepth3D.h b/libs/viz/include/mrpt/viz/CEllipsoidInverseDepth3D.h new file mode 100644 index 0000000000..588d3a88e9 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CEllipsoidInverseDepth3D.h @@ -0,0 +1,73 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +namespace mrpt::viz +{ +/** An especial "ellipsoid" in 3D computed as the uncertainty iso-surfaces of a + * (inv_range,yaw,pitch) variable. + * The parameter space of this ellipsoid comprises these variables (in this + * order): + * - inv_range: The inverse distance from the sensor to the feature. + * - yaw: Angle for the rotation around +Z ("azimuth"). + * - pitch: Angle for the rotation around +Y ("elevation"). Positive means + * pointing below the XY plane. + * + * This parameterization is based on the paper: + * - Civera, J. and Davison, A.J. and Montiel, J., "Inverse depth + * parametrization for monocular SLAM", T-RO, 2008. + * + * This class expects you to provide a mean vector of length 3 and a 3x3 + * covariance matrix, set with \a setCovMatrixAndMean(). + * + * Please read the documentation of + * CGeneralizedEllipsoidTemplate::setQuantiles() for learning + * the mathematical details about setting the desired confidence interval. + * + * ![mrpt::viz::CEllipsoidInverseDepth3D](preview_CEllipsoidInverseDepth3D.png) + * + * \ingroup mrpt_viz_grp + */ +class CEllipsoidInverseDepth3D : + public CGeneralizedEllipsoidTemplate<3>, + virtual public CVisualObject +{ + using BASE = CGeneralizedEllipsoidTemplate<3>; + DEFINE_SERIALIZABLE(CEllipsoidInverseDepth3D, mrpt::viz) + + public: + /** The maximum range to be used as a correction when a point of the + * ellipsoid falls in the negative ranges (default: 1e6) */ + void setUnderflowMaxRange(const float maxRange) { m_underflowMaxRange = maxRange; } + float getUnderflowMaxRange() const { return m_underflowMaxRange; } + + protected: + /** To be implemented by derived classes: maps, using some arbitrary space + * transformation, a list of points + * defining an ellipsoid in parameter space into their corresponding + * points in 2D/3D space. + */ + void transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const override; + + private: + float m_underflowMaxRange = 1e6f; + + public: + /** Constructor + */ + CEllipsoidInverseDepth3D() = default; + /** Private, virtual destructor: only can be deleted from smart pointers */ + ~CEllipsoidInverseDepth3D() override = default; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CEllipsoidRangeBearing2D.h b/libs/viz/include/mrpt/viz/CEllipsoidRangeBearing2D.h new file mode 100644 index 0000000000..df784305bb --- /dev/null +++ b/libs/viz/include/mrpt/viz/CEllipsoidRangeBearing2D.h @@ -0,0 +1,57 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +namespace mrpt::viz +{ +/** An especial "ellipsoid" in 2D computed as the uncertainty iso-surfaces of a + * (range,bearing) variable. + * The parameter space of this ellipsoid comprises these variables (in this + * order): + * - range: Distance from sensor to feature. + * - bearing: Angle from +X to the line that goes from the sensor towards the + * feature. + * + * This class expects you to provide a mean vector of length 2 and a 2x2 + * covariance matrix, set with \a setCovMatrixAndMean(). + * + * Please read the documentation of + * CGeneralizedEllipsoidTemplate::setQuantiles() for learning + * the mathematical details about setting the desired confidence interval. + * + * ![mrpt::viz::CEllipsoidRangeBearing2D](preview_CEllipsoidRangeBearing2D.png) + * + * \ingroup mrpt_viz_grp + */ +class CEllipsoidRangeBearing2D : + public CGeneralizedEllipsoidTemplate<2>, + virtual public CVisualObject +{ + using BASE = CGeneralizedEllipsoidTemplate<2>; + DEFINE_SERIALIZABLE(CEllipsoidRangeBearing2D, mrpt::viz) + protected: + /** To be implemented by derived classes: maps, using some arbitrary space + * transformation, a list of points + * defining an ellipsoid in parameter space into their corresponding + * points in 2D/3D space. + */ + void transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const override; + /** Constructor + */ + public: + CEllipsoidRangeBearing2D() = default; + /** Private, virtual destructor: only can be deleted from smart pointers */ + ~CEllipsoidRangeBearing2D() override = default; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CFrustum.h b/libs/viz/include/mrpt/viz/CFrustum.h new file mode 100644 index 0000000000..cb70a49f6d --- /dev/null +++ b/libs/viz/include/mrpt/viz/CFrustum.h @@ -0,0 +1,126 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include + +namespace mrpt::viz +{ +/** A solid or wireframe frustum in 3D (a rectangular truncated pyramid), with + * arbitrary (possibly assymetric) field-of-view angles. + * + * You can switch whether to show only the lines, the surface of the frustum, + * or both. + * By default only the lines are drawn. + * + * The color of the object (via CVisualObject::setColor()) affects the color + * of lines. + * To set the color of planes use \a setPlaneColor() + * + * As usual in MRPT, the +X axis is assumed to by the main direction, in this + * case of the pyramid axis. + * + * The horizontal and vertical FOVs can be set directly with \a setHorzFOV() + * and \a setVertFOV() if + * they are symmetric, or with \a setHorzFOVAsymmetric() and \a + * setVertFOVAsymmetric() otherwise. + * + * All FOV angles are positive numbers. FOVs must be below 90deg on each side + * (below 180deg in total). + * If you try to set FOVs to larger values they'll truncated to 89.9deg. + * + * + * ![mrpt::viz::CFrustum](preview_CFrustum.png) + * + * \sa mrpt::viz::Scene, mrpt::viz::CRenderizable + * \ingroup mrpt_viz_grp + */ +class CFrustum : + virtual public CVisualObject, + virtual public VisualObjectParams_Triangles, + virtual public VisualObjectParams_Lines +{ + DEFINE_SERIALIZABLE(CFrustum, mrpt::viz) + + public: + /** Changes the color of the planes; to change color of lines, use + * CRenderizable base methods. */ + void setPlaneColor(const mrpt::img::TColor& c) + { + m_planes_color = c; + CVisualObject::notifyChange(); + } + const mrpt::img::TColor& getPlaneColor() const { return m_planes_color; } + + /** Changes distance of near & far planes */ + void setNearFarPlanes(const float near_distance, const float far_distance); + + float getNearPlaneDistance() const { return m_min_distance; } + float getFarPlaneDistance() const { return m_max_distance; } + /** Changes horizontal FOV (symmetric) */ + void setHorzFOV(const float fov_horz_degrees); + /** Changes vertical FOV (symmetric) */ + void setVertFOV(const float fov_vert_degrees); + /** Changes horizontal FOV (asymmetric) */ + void setHorzFOVAsymmetric(const float fov_horz_left_degrees, const float fov_horz_right_degrees); + /** Changes vertical FOV (asymmetric) */ + void setVertFOVAsymmetric(const float fov_vert_down_degrees, const float fov_vert_up_degrees); + + float getHorzFOV() const { return mrpt::RAD2DEG(m_fov_horz_left + m_fov_horz_right); } + float getVertFOV() const { return mrpt::RAD2DEG(m_fov_vert_down + m_fov_vert_up); } + float getHorzFOVLeft() const { return mrpt::RAD2DEG(m_fov_horz_left); } + float getHorzFOVRight() const { return mrpt::RAD2DEG(m_fov_horz_right); } + float getVertFOVDown() const { return mrpt::RAD2DEG(m_fov_vert_down); } + float getVertFOVUp() const { return mrpt::RAD2DEG(m_fov_vert_up); } + + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Basic empty constructor. Set all parameters to default. */ + CFrustum(); + /** Constructor with some parameters */ + CFrustum( + float near_distance, + float far_distance, + float horz_FOV_degrees, + float vert_FOV_degrees, + float lineWidth, + bool draw_lines, + bool draw_planes); + + /** Constructor from camera intrinsic parameters: creates a frustrum with + * the correct vertical and horizontal FOV angles for the given camera + * model, in wireframe. + * + * \param intrinsics Camera intrinsics. Distortion is ignored here. + * \param focalDistScale Scale for the far plane, in meters/pixels. + * + * \note (New in MRPT 2.1.8) + */ + CFrustum(const mrpt::img::TCamera& intrinsics, double focalDistScale = 5e-3); + + /** Destructor */ + ~CFrustum() override = default; + + protected: + /** Near and far planes */ + float m_min_distance{0.1f}, m_max_distance{1.f}; + /** Semi FOVs (in radians) */ + float m_fov_horz_left, m_fov_horz_right; + /** Semi FOVs (in radians) */ + float m_fov_vert_down, m_fov_vert_up; + bool m_draw_lines{true}, m_draw_planes{false}; + mrpt::img::TColor m_planes_color; + + // Compute the 8 corners of the frustum: + std::array computeFrustumCorners() const; +}; +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CGeneralizedEllipsoidTemplate.h b/libs/viz/include/mrpt/viz/CGeneralizedEllipsoidTemplate.h new file mode 100644 index 0000000000..4fca47e6ab --- /dev/null +++ b/libs/viz/include/mrpt/viz/CGeneralizedEllipsoidTemplate.h @@ -0,0 +1,278 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include // for >> ops +#include // for >> ops +#include + +#include +#include + +namespace mrpt::viz +{ +/** A class that generalizes the concept of an ellipsoid to arbitrary + * parameterizations of + * uncertainty shapes in either 2D or 3D. See derived classes for examples. + * + * Please read the documentation of + * CGeneralizedEllipsoidTemplate::setQuantiles() for learning + * the mathematical details about setting the desired confidence interval. + * + * The main method to set the modeled uncertainty is \a setCovMatrixAndMean() + * + * \tparam DIM The dimensionality of the parameter space, which must coincide + * with that of the rendering space (2 or 3) + * + * By default, only the front faces are rendered. Use cullFaces() to change if + * needed. + * + * \ingroup mrpt_viz_grp + */ +template +class CGeneralizedEllipsoidTemplate : virtual public CVisualObject, public VisualObjectParams_Lines +{ + public: + /** The type of fixed-size covariance matrices for this representation + */ + using cov_matrix_t = mrpt::math::CMatrixFixed; + /** The type of fixed-size vector for this representation */ + using mean_vector_t = mrpt::math::CMatrixFixed; + + using array_parameter_t = mrpt::math::CMatrixFixed; + using array_point_t = mrpt::math::CMatrixFixed; + + /** Set the NxN covariance matrix that will determine the aspect of the + * ellipsoid - Notice that the + * covariance determines the uncertainty in the parameter space, which + * would be transformed by derived function + */ + template + void setCovMatrixAndMean(const MATRIX& new_cov, const VECTOR& new_mean) + { + MRPT_START + std::unique_lock lckWrite(m_ellipsoidDataMtx.data); + + ASSERT_EQUAL_(new_cov.cols(), new_cov.rows()); + ASSERT_EQUAL_(new_cov.cols(), DIM); + m_cov = new_cov; + m_mean = new_mean; + m_needToRecomputeEigenVals = true; + CVisualObject::notifyChange(); + MRPT_END + } + + /** Gets the current uncertainty covariance of parameter space */ + cov_matrix_t getCovMatrix() const + { + std::shared_lock lckRead(m_ellipsoidDataMtx.data); + return m_cov; + } + + /** Like setCovMatrixAndMean(), for mean=zero. + */ + template + void setCovMatrix(const MATRIX& new_cov) + { + setCovMatrixAndMean(new_cov, mean_vector_t::Zero()); + } + + /** Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid + *(default=3, ~97 or ~98% CI); the exact mathematical meaning is: + * This value of "quantiles" \a q should be set to the square root of + *the chi-squared inverse cdf corresponding to the desired confidence + *interval. Note that this value depends on the dimensionality. + * Refer to the MATLAB functions \a chi2inv() and \a chi2cdf(). + * + * Some common values follow here for the convenience of users: + * - Dimensionality=3 (3D ellipsoids): + * - 19.8748% CI -> q=1 + * - 73.8536% CI -> q=2 + * - 97.0709% CI -> q=3 + * - 99.8866% CI -> q=4 + * - Dimensionality=2 (2D ellipses): + * - 39.347% CI -> q=1 + * - 86.466% CI -> q=2 + * - 98.8891% CI -> q=3 + * - 99.9664% CI -> q=4 + * - Dimensionality=1 (Not aplicable to this class but provided for + *reference): + * - 68.27% CI -> q=1 + * - 95.45% CI -> q=2 + * - 99.73% CI -> q=3 + * - 99.9937% CI -> q=4 + * + */ + void setQuantiles(float q) + { + std::unique_lock lckWrite(m_ellipsoidDataMtx.data); + m_quantiles = q; + CVisualObject::notifyChange(); + } + /** Refer to documentation of \a setQuantiles() */ + float getQuantiles() const + { + std::shared_lock lckRead(m_ellipsoidDataMtx.data); + return m_quantiles; + } + + /** Set the number of segments of the surface/curve (higher means with + * greater resolution) */ + void setNumberOfSegments(const uint32_t numSegments) + { + std::unique_lock lckWrite(m_ellipsoidDataMtx.data); + m_numSegments = numSegments; + CVisualObject::notifyChange(); + } + uint32_t getNumberOfSegments() const + { + std::shared_lock lckRead(m_ellipsoidDataMtx.data); + return m_numSegments; + } + + /** If set to "true", a whole ellipsoid surface will be drawn, or if + * set to "false" (default) it will be drawn as a "wireframe". */ + void enableDrawSolid3D(bool v) + { + std::unique_lock lckWrite(m_ellipsoidDataMtx.data); + m_drawSolid3D = v; + CVisualObject::notifyChange(); + } + + /** Ray tracing */ + virtual bool traceRay( + [[maybe_unused]] const mrpt::poses::CPose3D& o, [[maybe_unused]] double& dist) const override + { + THROW_EXCEPTION("Not implemented "); + } + + protected: + /** To be implemented by derived classes: maps, using some arbitrary + * space transformation, a list of points defining an ellipsoid in + * parameter space into their corresponding points in 2D/3D space. + */ + virtual void transformFromParameterSpace( + const std::vector& params_pts, std::vector& out_pts) const = 0; + + mutable mrpt::containers::NonCopiableData m_ellipsoidDataMtx; + + mutable cov_matrix_t m_cov; + mean_vector_t m_mean; + mutable bool m_needToRecomputeEigenVals{true}; + + /** The number of "sigmas" for drawing the ellipse/ellipsoid (default=3) + */ + float m_quantiles{3.f}; + /** Number of segments in 2D/3D ellipsoids (default=20) */ + uint32_t m_numSegments{20}; + + /// bounding boxes, in object local coordinates + mutable mrpt::math::TPoint3D m_bb_min{0, 0, 0}, m_bb_max{0, 0, 0}; + + /** If set to "true", a whole ellipsoid surface will be drawn, or + * if set to "false" (default)it will be drawn as a "wireframe". */ + bool m_drawSolid3D{false}; + + /** Cholesky U triangular matrix cache. */ + mutable cov_matrix_t m_U; + mutable std::vector m_render_pts; + + void thisclass_writeToStream(mrpt::serialization::CArchive& out) const + { + using namespace mrpt::math; + + const uint8_t version = 0; + out << version << m_cov << m_mean << m_quantiles << m_numSegments; + } + void thisclass_readFromStream(mrpt::serialization::CArchive& in) + { + uint8_t version; + in >> version; + switch (version) + { + case 0: + { + in >> m_cov >> m_mean >> m_quantiles >> m_numSegments; + m_needToRecomputeEigenVals = true; + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); + } + + CGeneralizedEllipsoidTemplate() = default; + virtual ~CGeneralizedEllipsoidTemplate() override = default; + + // Uscaled, m_mean, params_pts, m_numSegments, m_numSegments + void generatePoints(const cov_matrix_t& U, std::vector& out_params_pts) const; + + /// When called, if m_needToRecomputeEigenVals==true, the points in + /// m_render_pts will be regenerated. + void recomputeRenderPoints() const + { + std::shared_lock lckRead(m_ellipsoidDataMtx.data); + + // 1) Update eigenvectors/values: + if (m_needToRecomputeEigenVals) + { + m_needToRecomputeEigenVals = false; + // Handle the special case of an ellipsoid of volume = 0 + const double d = m_cov.det(); + // Note: "d!=d" is a great test for invalid numbers, don't remove! + if (std::abs(d) < 1e-20 || d != d) + { + // All zeros: + m_U.setZero(DIM, DIM); + } + else + { + // A valid matrix: + m_cov.chol(m_U); + } + } + + // 2) Generate "standard" ellipsoid: + std::vector params_pts; + cov_matrix_t Uscaled = m_U; + Uscaled *= static_cast(m_quantiles); + generatePoints(Uscaled, params_pts); + + // 3) Transform into 2D/3D render space: + this->transformFromParameterSpace(params_pts, m_render_pts); + + // 3.5) Save bounding box: + m_bb_min = mrpt::math::TPoint3D( + std::numeric_limits::max(), std::numeric_limits::max(), 0); + m_bb_max = mrpt::math::TPoint3D( + -std::numeric_limits::max(), -std::numeric_limits::max(), 0); + for (size_t i = 0; i < m_render_pts.size(); i++) + for (int k = 0; k < DIM; k++) + { + mrpt::keep_min(m_bb_min[k], m_render_pts[i][k]); + mrpt::keep_max(m_bb_max[k], m_render_pts[i][k]); + } + } + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override + { + return {m_bb_min, m_bb_max}; + } + + public: + // Solve virtual public inheritance ambiguity: + virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const override + { + return CVisualObject::GetRuntimeClass(); + } +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CGridPlaneXY.h b/libs/viz/include/mrpt/viz/CGridPlaneXY.h new file mode 100644 index 0000000000..97c8a38310 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CGridPlaneXY.h @@ -0,0 +1,82 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include + +namespace mrpt::viz +{ +/** A grid of lines over the XY plane. + * + * ![mrpt::viz::CGridPlaneXY](preview_CGridPlaneXY.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CGridPlaneXY : virtual public CVisualObject, public VisualObjectParams_Lines +{ + DEFINE_SERIALIZABLE(CGridPlaneXY, mrpt::viz) + + protected: + float m_xMin, m_xMax; + float m_yMin, m_yMax; + float m_plane_z; + float m_frequency; + + public: + void setPlaneLimits(float xmin, float xmax, float ymin, float ymax) + { + m_xMin = xmin; + m_xMax = xmax; + m_yMin = ymin; + m_yMax = ymax; + CVisualObject::notifyChange(); + } + + void getPlaneLimits(float& xmin, float& xmax, float& ymin, float& ymax) const + { + xmin = m_xMin; + xmax = m_xMax; + ymin = m_yMin; + ymax = m_yMax; + } + + void setPlaneZcoord(float z) + { + CVisualObject::notifyChange(); + m_plane_z = z; + } + float getPlaneZcoord() const { return m_plane_z; } + void setGridFrequency(float freq) + { + ASSERT_(freq > 0); + m_frequency = freq; + CVisualObject::notifyChange(); + } + float getGridFrequency() const { return m_frequency; } + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Constructor */ + CGridPlaneXY( + float xMin = -10, + float xMax = 10, + float yMin = -10, + float yMax = 10, + float z = 0, + float frequency = 1, + float lineWidth = 1.3f, + bool antiAliasing = true); + + /** Private, virtual destructor: only can be deleted from smart pointers */ + ~CGridPlaneXY() override = default; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CGridPlaneXZ.h b/libs/viz/include/mrpt/viz/CGridPlaneXZ.h new file mode 100644 index 0000000000..6da596a7b4 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CGridPlaneXZ.h @@ -0,0 +1,81 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include + +namespace mrpt::viz +{ +/** A grid of lines over the XZ plane. + * + * ![mrpt::viz::CGridPlaneXZ](preview_CGridPlaneXZ.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CGridPlaneXZ : virtual public CVisualObject, public VisualObjectParams_Lines +{ + DEFINE_SERIALIZABLE(CGridPlaneXZ, mrpt::viz) + + protected: + float m_xMin, m_xMax; + float m_zMin, m_zMax; + float m_plane_y; + float m_frequency; + + public: + void setPlaneLimits(float xmin, float xmax, float zmin, float zmax) + { + m_xMin = xmin; + m_xMax = xmax; + m_zMin = zmin; + m_zMax = zmax; + CVisualObject::notifyChange(); + } + + void getPlaneLimits(float& xmin, float& xmax, float& zmin, float& zmax) const + { + xmin = m_xMin; + xmax = m_xMax; + zmin = m_zMin; + zmax = m_zMax; + } + + void setPlaneYcoord(float y) + { + m_plane_y = y; + CVisualObject::notifyChange(); + } + float getPlaneYcoord() const { return m_plane_y; } + void setGridFrequency(float freq) + { + ASSERT_(freq > 0); + m_frequency = freq; + CVisualObject::notifyChange(); + } + float getGridFrequency() const { return m_frequency; } + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Constructor */ + CGridPlaneXZ( + float xMin = -10, + float xMax = 10, + float zMin = -10, + float zMax = 10, + float y = 0, + float frequency = 1, + float lineWidth = 1.3f, + bool antiAliasing = true); + /** Private, virtual destructor: only can be deleted from smart pointers */ + ~CGridPlaneXZ() override = default; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CMesh.h b/libs/viz/include/mrpt/viz/CMesh.h new file mode 100644 index 0000000000..3af43cc5f2 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CMesh.h @@ -0,0 +1,260 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace mrpt::viz +{ +/** A planar (XY) grid where each cell has an associated height and, optionally, + * a texture map. A typical usage example would be an elevation map or a 3D + * model of a terrain. + * + * The height of each cell/pixel is provided via an elevation `Z` matrix, + * where the z coordinate of the grid cell (x,y) is given by `Z(x,y)` + * (not `Z(y,x)`!!), that is: + * - Z column count = number of cells in direction "+y" + * - Z row count = number of cells in direction "+x" + * + * Since MRPT 2.7.0, the texture can be wrapped over the mesh using + * setMeshTextureExtension(). + * + * ![mrpt::viz::CMesh](preview_CMesh.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CMesh : + virtual public CVisualObject, + public VisualObjectParams_TexturedTriangles, + public VisualObjectParams_Lines +{ + DEFINE_SERIALIZABLE(CMesh, mrpt::viz) + public: + struct TTriangleVertexIndices + { + size_t vind[3] = {0, 0, 0}; + }; + + public: + CMesh( + bool enableTransparency = false, + float xMin = -1.0f, + float xMax = 1.0f, + float yMin = -1.0f, + float yMax = 1.0f); + + virtual ~CMesh() override; + + template + void setGridLimits(T xMin, T xMax, T yMin, T yMax) + { + m_xMin = static_cast(xMin); + m_xMax = static_cast(xMax); + m_yMin = static_cast(yMin); + m_yMax = static_cast(yMax); + CVisualObject::notifyChange(); + } + + void getGridLimits(float& xMin, float& xMax, float& yMin, float& yMax) const + { + xMin = m_xMin; + xMax = m_xMax; + yMin = m_yMin; + yMax = m_yMax; + } + + /** Sets the texture physical size (in "meters) using to wrap it over the + * mesh extension. + * The default (0) means texture size is equal to whole grid extension. */ + void setMeshTextureExtension(float textureSize_x, float textureSize_y) + { + m_textureSize_x = textureSize_x; + m_textureSize_y = textureSize_y; + CVisualObject::notifyChange(); + } + + void getMeshTextureExtension(float& textureSize_x, float& textureSize_y) const + { + textureSize_x = m_textureSize_x; + textureSize_y = m_textureSize_y; + } + + void enableTransparency(bool v) + { + m_enableTransparency = v; + CVisualObject::notifyChange(); + } + void enableWireFrame(bool v) + { + m_isWireFrame = v; + CVisualObject::notifyChange(); + } + void enableColorFromZ(bool v, mrpt::img::TColormap colorMap = mrpt::img::cmHOT) + { + m_colorFromZ = v; + m_colorMap = colorMap; + CVisualObject::notifyChange(); + } + + /** This method sets the matrix of heights for each position (cell) in the + * mesh grid */ + void setZ(const mrpt::math::CMatrixDynamic& in_Z); + + /** Returns a reference to the internal Z matrix, allowing changing it + * efficiently */ + void getZ(mrpt::math::CMatrixFloat& out) const { out = Z; } + /** Returns a reference to the internal mask matrix, allowing changing it + * efficiently */ + void getMask(mrpt::math::CMatrixFloat& out) const { out = mask; } + /** This method sets the boolean mask of valid heights for each position + * (cell) in the mesh grid */ + void setMask(const mrpt::math::CMatrixDynamic& in_mask); + + float getxMin() const { return m_xMin; } + float getxMax() const { return m_xMax; } + float getyMin() const { return m_yMin; } + float getyMax() const { return m_yMax; } + void setxMin(const float nxm) + { + m_xMin = nxm; + m_trianglesUpToDate = false; + CVisualObject::notifyChange(); + } + void setxMax(const float nxm) + { + m_xMax = nxm; + m_trianglesUpToDate = false; + CVisualObject::notifyChange(); + } + void setyMin(const float nym) + { + m_yMin = nym; + m_trianglesUpToDate = false; + CVisualObject::notifyChange(); + } + void setyMax(const float nym) + { + m_yMax = nym; + m_trianglesUpToDate = false; + CVisualObject::notifyChange(); + } + void getXBounds(float& min, float& max) const + { + min = m_xMin; + max = m_xMax; + } + void getYBounds(float& min, float& max) const + { + min = m_yMin; + max = m_yMax; + } + void setXBounds(const float min, const float max) + { + m_xMin = min; + m_xMax = max; + m_trianglesUpToDate = false; + CVisualObject::notifyChange(); + } + void setYBounds(const float min, const float max) + { + m_yMin = min; + m_yMax = max; + m_trianglesUpToDate = false; + CVisualObject::notifyChange(); + } + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Assigns a texture image. + */ + void assignImage(const mrpt::img::CImage& img); + + /** Assigns a texture image and Z simultaneously, and disable transparency. + */ + void assignImageAndZ(const mrpt::img::CImage& img, const mrpt::math::CMatrixDynamic& in_Z); + + /** Adjust grid limits according to the image aspect ratio, maintaining the + * X limits and resizing in the Y direction. + */ + void adjustGridToImageAR(); + + /** Trace ray + */ + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + + protected: + bool m_enableTransparency; + bool m_colorFromZ{false}; + bool m_isWireFrame{false}; + bool m_isImage{false}; + + /** Z(x,y): Z-coordinate of the point (x,y) */ + math::CMatrixF Z; + math::CMatrixF mask; + + /** Grayscale Color [0,1] for each cell, updated by updateColorsMatrix */ + mutable math::CMatrixF C; + /** Red Component of the Color [0,1] for each cell, updated by + * updateColorsMatrix */ + mutable math::CMatrixF C_r; + /** Green Component of the Color [0,1] for each cell, updated by + * updateColorsMatrix */ + mutable math::CMatrixF C_g; + /** Blue Component of the Color [0,1] for each cell, updated by + * updateColorsMatrix */ + mutable math::CMatrixF C_b; + + /** Used when m_colorFromZ is true */ + mrpt::img::TColormap m_colorMap{mrpt::img::cmHOT}; + + /** Whether C is not up-to-date wrt to Z */ + mutable bool m_modified_Z{true}; + /** Whether C is not up-to-date wrt to the texture image */ + mutable bool m_modified_Image{false}; + + /** Called internally to assure C is updated. */ + void updateColorsMatrix() const; + /** Called internally to assure the triangle list is updated. */ + void updateTriangles() const; + void updatePolygons() const; // m_meshDataMtx; + + /** List of triangles in the mesh */ + mutable std::vector> actualMesh; + + /** The accumulated normals & counts for each vertex, so normals can be + * averaged. */ + mutable std::vector> vertex_normals; + /**Whether the actual mesh needs to be recalculated */ + mutable bool m_trianglesUpToDate{false}; + /**Whether the polygon mesh (auxiliary structure for ray tracing) needs to + * be recalculated */ + mutable bool m_polygonsUpToDate{false}; + mutable std::vector tmpPolys; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CMesh3D.h b/libs/viz/include/mrpt/viz/CMesh3D.h new file mode 100644 index 0000000000..871c584e2d --- /dev/null +++ b/libs/viz/include/mrpt/viz/CMesh3D.h @@ -0,0 +1,141 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include +#include +#include + +namespace mrpt::viz +{ +/** A 3D mesh composed of triangles and/or quads. + * A typical usage example would be a 3D model of an object. + * + * ![mrpt::viz::CMesh3D](preview_CMesh3D.png) + * + * \sa opengl::Scene,opengl::CMesh,opengl::CAssimpModel + * \ingroup mrpt_viz_grp + */ +class CMesh3D : + virtual public CVisualObject, + public VisualObjectParams_Lines, + public VisualObjectParams_Triangles +{ + DEFINE_SERIALIZABLE(CMesh3D, mrpt::viz) + + public: + CMesh3D() = default; + virtual ~CMesh3D() override; + + void enableShowEdges(bool v) + { + m_showEdges = v; + CVisualObject::notifyChange(); + } + void enableShowFaces(bool v) + { + m_showFaces = v; + CVisualObject::notifyChange(); + } + void enableShowVertices(bool v) + { + m_showVertices = v; + CVisualObject::notifyChange(); + } + void enableFaceNormals(bool v) + { + m_computeNormals = v; + CVisualObject::notifyChange(); + } + + /** Load a 3D mesh. The arguments indicate: + - num_verts: Number of vertices of the mesh + - num_faces: Number of faces of the mesh + - verts_per_face: An array (pointer) with the number of vertices of each + face. The elements must be set either to 3 (triangle) or 4 (quad). + - face_verts: An array (pointer) with the vertices of each face. The + vertices of each face must be consecutive in this array. + - vert_coords: An array (pointer) with the coordinates of each vertex. + The xyz coordinates of each vertex must be consecutive in this array. + */ + void loadMesh( + unsigned int num_verts, + unsigned int num_faces, + int* verts_per_face, + int* face_verts, + float* vert_coords); + + /** Load a 3D mesh. The arguments indicate: + - num_verts: Number of vertices of the mesh + - num_faces: Number of faces of the mesh + - is_quad: A binary array saying whether the face is a quad (1) + or a triangle (0) + - face_verts: An array with the vertices of each face. For every + column (face), each row contains the num of a vertex. The fourth does not + need + to be filled if the face is a triangle. + - vert_coords: An array with the coordinates of each vertex. For + every column (vertex), each row contains the xyz coordinates of the + vertex. + */ + void loadMesh( + unsigned int num_verts, + unsigned int num_faces, + const mrpt::math::CMatrixDynamic& is_quad, + const mrpt::math::CMatrixDynamic& face_verts, + const mrpt::math::CMatrixDynamic& vert_coords); + + void setEdgeColor(float r, float g, float b, float a = 1.f) + { + edge_color = mrpt::img::TColorf(r, g, b, a); + } + void setFaceColor(float r, float g, float b, float a = 1.f) + { + face_color = mrpt::img::TColorf(r, g, b, a); + } + void setVertColor(float r, float g, float b, float a = 1.f) + { + vert_color = mrpt::img::TColorf(r, g, b, a); + } + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + protected: + using vertex_indices_t = mrpt::math::CMatrixFixed; + + bool m_showEdges = true; + bool m_showFaces = true; + bool m_showVertices = false; + bool m_computeNormals = true; + + /** Pointer storing whether a face is a quad (1) or a triangle (0) */ + std::vector m_is_quad; + /** Pointer storing the vertices that compose each face. Size: 4 x num_faces + * (4 for the possible max number - quad) */ + std::vector m_face_verts; + + /** Pointer storing the coordinates of the vertices. Size: 3 x num_vertices + */ + std::vector m_vertices; + + /** Pointer storing the face normals. Size: 3 x num_faces */ + std::vector m_normals; + + /** Color of the edges */ + mrpt::img::TColorf edge_color = {.9f, .9f, .9f, 1.0f}; + + /** Color of the faces */ + mrpt::img::TColorf face_color = {.7f, .7f, .8f, 1.0f}; + + /** Color of the vertices */ + mrpt::img::TColorf vert_color = {.3f, .3f, .3f, 1.0f}; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CMeshFast.h b/libs/viz/include/mrpt/viz/CMeshFast.h new file mode 100644 index 0000000000..4cdb5b8599 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CMeshFast.h @@ -0,0 +1,212 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include +#include +#include +#include + +namespace mrpt::viz +{ +/** A planar (XY) grid where each cell has an associated height and, optionally, + * a texture map. + * To make it faster to render, instead of drawing lines and triangles it draws + * a point at each gridcell. + * A typical usage example would be an elevation map or a 3D model of a + * terrain. + * + * ![mrpt::viz::CMeshFast](preview_CMeshFast.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CMeshFast : virtual public CVisualObject, public VisualObjectParams_Points +{ + DEFINE_SERIALIZABLE(CMeshFast, mrpt::viz) + + public: + /** Constructor + */ + CMeshFast( + bool enableTransparency = false, + float xMin_p = -1.0f, + float xMax_p = 1.0f, + float yMin_p = -1.0f, + float yMax_p = 1.0f) : + m_textureImage(0, 0), + m_enableTransparency(enableTransparency), + X(0, 0), + Y(0, 0), + Z(0, 0), + C(0, 0), + C_r(0, 0), + C_g(0, 0), + C_b(0, 0), + xMin(xMin_p), + xMax(xMax_p), + yMin(yMin_p), + yMax(yMax_p) + { + setColor_u8(0, 0, 150); + } + virtual ~CMeshFast() override = default; + + void setGridLimits(float xmin, float xmax, float ymin, float ymax) + { + xMin = xmin; + xMax = xmax; + yMin = ymin; + yMax = ymax; + CVisualObject::notifyChange(); + } + + void getGridLimits(float& xmin, float& xmax, float& ymin, float& ymax) const + { + xmin = xMin; + xmax = xMax; + ymin = yMin; + ymax = yMax; + } + + void enableTransparency(bool v) + { + m_enableTransparency = v; + CVisualObject::notifyChange(); + } + void enableColorFromZ(bool v, mrpt::img::TColormap colorMap = mrpt::img::cmJET) + { + m_colorFromZ = v; + m_colorMap = colorMap; + CVisualObject::notifyChange(); + } + + /** This method sets the matrix of heights for each position (cell) in the + * mesh grid */ + void setZ(const mrpt::math::CMatrixDynamic& in_Z); + + /** Returns a reference to the internal Z matrix, allowing changing it + * efficiently */ + void getZ(mrpt::math::CMatrixFloat& out) const { out = Z; } + float getXMin() const { return xMin; } + float getXMax() const { return xMax; } + float getYMin() const { return yMin; } + float getYMax() const { return yMax; } + void setXMin(float nxm) + { + xMin = nxm; + pointsUpToDate = false; + CVisualObject::notifyChange(); + } + void setXMax(float nxm) + { + xMax = nxm; + pointsUpToDate = false; + CVisualObject::notifyChange(); + } + void setYMin(float nym) + { + yMin = nym; + pointsUpToDate = false; + CVisualObject::notifyChange(); + } + void setYMax(float nym) + { + yMax = nym; + pointsUpToDate = false; + CVisualObject::notifyChange(); + } + void getXBounds(float& min, float& max) const + { + min = xMin; + max = xMax; + } + void getYBounds(float& min, float& max) const + { + min = yMin; + max = yMax; + } + void setXBounds(float min, float max) + { + xMin = min; + xMax = max; + pointsUpToDate = false; + CVisualObject::notifyChange(); + } + void setYBounds(float min, float max) + { + yMin = min; + yMax = max; + pointsUpToDate = false; + CVisualObject::notifyChange(); + } + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Assigns a texture image, and disable transparency. + */ + void assignImage(const mrpt::img::CImage& img); + + /** Assigns a texture image and Z simultaneously, and disable transparency. + */ + void assignImageAndZ(const mrpt::img::CImage& img, const mrpt::math::CMatrixDynamic& in_Z); + + /** Adjust grid limits according to the image aspect ratio, maintaining the + * X limits and resizing in the Y direction. + */ + void adjustGridToImageAR() + { + ASSERT_(m_isImage); + const float ycenter = 0.5f * (yMin + yMax); + const float xwidth = xMax - xMin; + const float newratio = float(m_textureImage.getWidth()) / float(m_textureImage.getHeight()); + yMax = ycenter + 0.5f * newratio * xwidth; + yMin = ycenter - 0.5f * newratio * xwidth; + CVisualObject::notifyChange(); + } + + protected: + mrpt::img::CImage m_textureImage; + + bool m_enableTransparency; + bool m_colorFromZ{false}; + bool m_isImage{false}; + + /** X(x,y): X-coordinate of the point (x,y) */ + mutable math::CMatrixF X; + /** Y(x,y): Y-coordinate of the point (x,y) */ + mutable math::CMatrixF Y; + /** Z(x,y): Z-coordinate of the point (x,y) */ + mutable math::CMatrixF Z; + + /** Grayscale or RGB components [0,255] for each cell, updated by + * updateColorsMatrix */ + mutable math::CMatrix_u8 C, C_r, C_g, C_b; + + /** Used when m_colorFromZ is true */ + mrpt::img::TColormap m_colorMap{mrpt::img::cmJET}; + + /** Whether C is not up-to-date wrt to Z */ + mutable bool m_modified_Z{true}; + /** Whether C is not up-to-date wrt to the texture image */ + mutable bool m_modified_Image{false}; + + /** Called internally to assure C is updated. */ + void updateColorsMatrix() const; + void updatePoints() const; + + /** Mesh bounds */ + float xMin, xMax, yMin, yMax; + + /**Whether the coordinates of the points needs to be recalculated */ + mutable bool pointsUpToDate{false}; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/COctoMapVoxels.h b/libs/viz/include/mrpt/viz/COctoMapVoxels.h new file mode 100644 index 0000000000..3c52c28273 --- /dev/null +++ b/libs/viz/include/mrpt/viz/COctoMapVoxels.h @@ -0,0 +1,336 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include + +namespace mrpt::viz +{ +enum predefined_voxel_sets_t +{ + VOXEL_SET_OCCUPIED = 0, + VOXEL_SET_FREESPACE = 1 +}; + +/** A flexible renderer of voxels, typically from a 3D octo map (see + *mrpt::maps::COctoMap). + * This class is sort of equivalent to octovis::OcTreeDrawer from the octomap + *package, but + * relying on MRPT's CRenderizable so there's no need to manually + *cache the rendering of OpenGL primitives. + * + * Normally users call mrpt::maps::COctoMap::getAs3DObject() to obtain a + *generic mrpt::viz::CSetOfObjects which insides holds an instance of + *COctoMapVoxels. + * You can also alternativelly call COctoMapVoxels::setFromOctoMap(), so you + *can tune the display parameters, colors, etc. + * As with any other mrpt::viz class, all object coordinates refer to some + *frame of reference which is relative to the object parent and can be changed + *with mrpt::viz::CVisualObject::setPose() + * + * This class draws these separate elements to represent an OctoMap: + * - A grid representation of all cubes, as simple lines (occupied/free, + *leafs/nodes,... whatever). See: + * - showGridLines() + * - setGridLinesColor() + * - setGridLinesWidth() + * - push_back_GridCube() + * - A number of voxel collections, drawn as cubes each having a + *different color (e.g. depending on the color scheme in the original + *mrpt::maps::COctoMap object). + * The meanning of each collection is user-defined, but you can use the + *constants VOXEL_SET_OCCUPIED, VOXEL_SET_FREESPACE for predefined meanings. + * - showVoxels() + * - push_back_Voxel() + * + * Several coloring schemes can be selected with setVisualizationMode(). See + *COctoMapVoxels::visualization_mode_t + * + * ![mrpt::viz::COctoMapVoxels](preview_COctoMapVoxels.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class COctoMapVoxels : + virtual public CVisualObject, + public VisualObjectParams_Lines, + public VisualObjectParams_Triangles, + public VisualObjectParams_Points +{ + DEFINE_SERIALIZABLE(COctoMapVoxels, mrpt::viz) + public: + /** The different coloring schemes, which modulate the generic + * mrpt::viz::CRenderizable object color. Set with setVisualizationMode() + */ + enum visualization_mode_t + { + /** Color goes from black (at the bottom) to the chosen color (at the + top) */ + COLOR_FROM_HEIGHT, + /** Color goes from black (occupied voxel) to the chosen color (free + voxel) */ + COLOR_FROM_OCCUPANCY, + /** Transparency goes from opaque (occupied voxel) to transparent (free + voxel). */ + TRANSPARENCY_FROM_OCCUPANCY, + /** Color goes from black (occupaid voxel) to the chosen color (free + voxel) and they are transparent */ + TRANS_AND_COLOR_FROM_OCCUPANCY, + /** Combination of COLOR_FROM_HEIGHT and TRANSPARENCY_FROM_OCCUPANCY */ + MIXED, + /** All cubes are of identical color. */ + FIXED, + /** Color from RGB data */ + COLOR_FROM_RGB_DATA + }; + + /** The info of each of the voxels */ + struct TVoxel + { + mrpt::math::TPoint3Df coords; + double side_length; + mrpt::img::TColor color; + + TVoxel() = default; + TVoxel( + const mrpt::math::TPoint3Df& coords_, const double side_length_, mrpt::img::TColor color_) : + coords(coords_), side_length(side_length_), color(color_) + { + } + }; + + /** The info of each grid block */ + struct TGridCube + { + /** opposite corners of the cube */ + mrpt::math::TPoint3Df min, max; + + TGridCube() = default; + TGridCube(const mrpt::math::TPoint3Df& min_, const mrpt::math::TPoint3Df& max_) : + min(min_), max(max_) + { + } + }; + + struct TInfoPerVoxelSet + { + bool visible{true}; + std::vector voxels; + + TInfoPerVoxelSet() = default; + }; + + mrpt::img::TColormap colorMap() const { return m_color_map; } + + /** Changing the colormap has no effect until a source object (e.g. + * mrpt::maps::CVoxelMap) reads this property while generating the voxels + * visualization. + */ + void colorMap(const mrpt::img::TColormap& cm) { m_color_map = cm; } + + protected: + std::deque m_voxel_sets; + std::vector m_grid_cubes; + + /** Cached bounding boxes */ + mrpt::math::TPoint3D m_bb_min, m_bb_max; + + bool m_enable_lighting{false}; + bool m_enable_cube_transparency{true}; + bool m_showVoxelsAsPoints{false}; + float m_showVoxelsAsPointsSize{3.0f}; + bool m_show_grids{false}; + float m_grid_width{1.0f}; + mrpt::img::TColor m_grid_color; + visualization_mode_t m_visual_mode{COctoMapVoxels::COLOR_FROM_OCCUPANCY}; + mrpt::img::TColormap m_color_map = mrpt::img::cmHOT; + + public: + /** Clears everything */ + void clear(); + + /** Select the visualization mode. To have any effect, this method has to be + * called before loading the octomap. */ + void setVisualizationMode(visualization_mode_t mode) + { + m_visual_mode = mode; + CVisualObject::notifyChange(); + } + visualization_mode_t getVisualizationMode() const { return m_visual_mode; } + + /** Can be used to enable/disable the effects of lighting in this object */ + void enableLights(bool enable) + { + m_enable_lighting = enable; + CVisualObject::notifyChange(); + } + bool areLightsEnabled() const { return m_enable_lighting; } + /** By default, the alpha (transparency) component of voxel cubes is taken + * into account, but transparency can be disabled with this method. */ + void enableCubeTransparency(bool enable) + { + m_enable_cube_transparency = enable; + CVisualObject::notifyChange(); + } + bool isCubeTransparencyEnabled() const { return m_enable_cube_transparency; } + + /** Shows/hides the grid lines */ + void showGridLines(bool show) + { + m_show_grids = show; + CVisualObject::notifyChange(); + } + bool areGridLinesVisible() const { return m_show_grids; } + /** Shows/hides the voxels (voxel_set is a 0-based index for the set of + * voxels to modify, e.g. VOXEL_SET_OCCUPIED, VOXEL_SET_FREESPACE) */ + void showVoxels(unsigned int voxel_set, bool show) + { + ASSERT_(voxel_set < m_voxel_sets.size()); + m_voxel_sets[voxel_set].visible = show; + CVisualObject::notifyChange(); + } + bool areVoxelsVisible(unsigned int voxel_set) const + { + ASSERT_(voxel_set < m_voxel_sets.size()); + return m_voxel_sets[voxel_set].visible; + } + + /** For quick renders: render voxels as points instead of cubes. \sa + * setVoxelAsPointsSize */ + void showVoxelsAsPoints(const bool enable) + { + m_showVoxelsAsPoints = enable; + CVisualObject::notifyChange(); + } + bool areVoxelsShownAsPoints() const { return m_showVoxelsAsPoints; } + /** Only used when showVoxelsAsPoints() is enabled. */ + void setVoxelAsPointsSize(float pointSize) + { + m_showVoxelsAsPointsSize = pointSize; + CVisualObject::notifyChange(); + } + float getVoxelAsPointsSize() const { return m_showVoxelsAsPointsSize; } + + /** Sets the width of grid lines */ + void setGridLinesWidth(float w) + { + m_grid_width = w; + CVisualObject::notifyChange(); + } + /** Gets the width of grid lines */ + float getGridLinesWidth() const { return m_grid_width; } + void setGridLinesColor(const mrpt::img::TColor& color) + { + m_grid_color = color; + CVisualObject::notifyChange(); + } + const mrpt::img::TColor& getGridLinesColor() const { return m_grid_color; } + + /** Returns the total count of grid cubes. */ + size_t getGridCubeCount() const { return m_grid_cubes.size(); } + /** Returns the number of voxel sets. */ + size_t getVoxelSetCount() const { return m_voxel_sets.size(); } + /** Returns the total count of voxels in one voxel set. */ + size_t getVoxelCount(size_t set_index) const + { + ASSERT_(set_index < m_voxel_sets.size()); + return m_voxel_sets[set_index].voxels.size(); + } + + /** Manually changes the bounding box (normally the user doesn't need to + * call this) */ + void setBoundingBox(const mrpt::math::TPoint3D& bb_min, const mrpt::math::TPoint3D& bb_max); + + void resizeGridCubes(size_t nCubes) + { + m_grid_cubes.resize(nCubes); + CVisualObject::notifyChange(); + } + void resizeVoxelSets(size_t nVoxelSets) + { + m_voxel_sets.resize(nVoxelSets); + CVisualObject::notifyChange(); + } + void resizeVoxels(size_t set_index, size_t nVoxels) + { + ASSERT_(set_index < m_voxel_sets.size()); + m_voxel_sets[set_index].voxels.resize(nVoxels); + CVisualObject::notifyChange(); + } + + void reserveGridCubes(size_t nCubes) { m_grid_cubes.reserve(nCubes); } + void reserveVoxels(size_t set_index, size_t nVoxels) + { + ASSERT_(set_index < m_voxel_sets.size()); + m_voxel_sets[set_index].voxels.reserve(nVoxels); + CVisualObject::notifyChange(); + } + + TGridCube& getGridCubeRef(size_t idx) + { + ASSERTDEB_(idx < m_grid_cubes.size()); + CVisualObject::notifyChange(); + return m_grid_cubes[idx]; + } + const TGridCube& getGridCube(size_t idx) const + { + ASSERTDEB_(idx < m_grid_cubes.size()); + return m_grid_cubes[idx]; + } + + TVoxel& getVoxelRef(size_t set_index, size_t idx) + { + ASSERTDEB_(set_index < m_voxel_sets.size() && idx < m_voxel_sets[set_index].voxels.size()); + CVisualObject::notifyChange(); + return m_voxel_sets[set_index].voxels[idx]; + } + const TVoxel& getVoxel(size_t set_index, size_t idx) const + { + ASSERTDEB_(set_index < m_voxel_sets.size() && idx < m_voxel_sets[set_index].voxels.size()); + CVisualObject::notifyChange(); + return m_voxel_sets[set_index].voxels[idx]; + } + + void push_back_GridCube(const TGridCube& c) + { + CVisualObject::notifyChange(); + m_grid_cubes.push_back(c); + } + void push_back_Voxel(size_t set_index, const TVoxel& v) + { + ASSERTDEB_(set_index < m_voxel_sets.size()); + CVisualObject::notifyChange(); + m_voxel_sets[set_index].voxels.push_back(v); + } + + void sort_voxels_by_z(); + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Sets the contents of the object from a mrpt::maps::COctoMap object. + * \tparam Typically, an mrpt::maps::COctoMap object + * + * \note Declared as a template because in the library [mrpt-opengl] we + * don't have access to the library [mrpt-maps]. + */ + template + void setFromOctoMap(OCTOMAP& m) + { + m.getAsOctoMapVoxels(*this); + } + + /** Constructor */ + COctoMapVoxels(); + /** Private, virtual destructor: only can be deleted from smart pointers. */ + ~COctoMapVoxels() override = default; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CPointCloud.h b/libs/viz/include/mrpt/viz/CPointCloud.h new file mode 100644 index 0000000000..54191a6983 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CPointCloud.h @@ -0,0 +1,362 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include +#include + +namespace mrpt::viz +{ +/** A cloud of points, all with the same color or each depending on its value + * along a particular coordinate axis. + * This class is just an OpenGL representation of a point cloud. For operating + * with maps of points, see mrpt::maps::CPointsMap and derived classes. + * + * To load from a points-map, CPointCloud::loadFromPointsMap(). + * + * This class uses smart optimizations while rendering to efficiently draw + * clouds of millions of points, using octrees. + * + * ![mrpt::viz::CPointCloud](preview_CPointCloud.png) + * + * \sa opengl::CPlanarLaserScan, mrpt::viz::Scene, + * mrpt::viz::CPointCloudColoured, mrpt::maps::CPointsMap + * \ingroup mrpt_viz_grp + */ +class CPointCloud : + virtual public CVisualObject, + public VisualObjectParams_Points, + public mrpt::viz::PLY_Importer, + public mrpt::viz::PLY_Exporter +{ + DEFINE_SERIALIZABLE(CPointCloud, mrpt::viz) + DEFINE_SCHEMA_SERIALIZABLE() + protected: + enum Axis + { + colNone = 0, + colZ, + colY, + colX + }; + + Axis m_colorFromDepth = CPointCloud::colNone; + + /** Actually, an alias for the base class shader container of points. Kept + * to have an easy to use name. */ + std::vector& m_points = VisualObjectParams_Points::m_vertex_buffer_data; + + /** Do needed internal work if all points are new (octree rebuilt,...) */ + void markAllPointsAsNew(); + + protected: + /** @name PLY Import virtual methods to implement in base classes + @{ */ + /** In a base class, reserve memory to prepare subsequent calls to + * PLY_import_set_vertex */ + void PLY_import_set_vertex_count(size_t N) override; + + void PLY_import_set_vertex_timestamp( + [[maybe_unused]] size_t idx, [[maybe_unused]] const double unixTimestamp) override + { + // do nothing, this class ignores timestamps + } + + /** In a base class, reserve memory to prepare subsequent calls to + * PLY_import_set_face */ + void PLY_import_set_face_count([[maybe_unused]] size_t N) override {} + + /** In a base class, will be called after PLY_import_set_vertex_count() once + * for each loaded point. + * \param pt_color Will be nullptr if the loaded file does not provide + * color info. + */ + void PLY_import_set_vertex( + size_t idx, + const mrpt::math::TPoint3Df& pt, + const mrpt::img::TColorf* pt_color = nullptr) override; + /** @} */ + + /** @name PLY Export virtual methods to implement in base classes + @{ */ + size_t PLY_export_get_vertex_count() const override; + size_t PLY_export_get_face_count() const override { return 0; } + void PLY_export_get_vertex( + size_t idx, + mrpt::math::TPoint3Df& pt, + bool& pt_has_color, + mrpt::img::TColorf& pt_color) const override; + /** @} */ + + auto internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf override; + + public: + /** @name Read/Write of the list of points to render + @{ */ + + size_t size() const + { + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + return m_points.size(); + } + /// Like size(), but without locking the data mutex (internal usage) + size_t size_unprotected() const { return m_points.size(); } + + /** Set the number of points (with contents undefined) */ + void resize(size_t N) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + m_points.resize(N); + m_minmax_valid = false; + wfWriteLock.unlock(); + markAllPointsAsNew(); + } + + /** Like STL std::vector's reserve */ + void reserve(size_t N) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + m_points.reserve(N); + } + + /** Set the list of (X,Y,Z) point coordinates, all at once, from three + * vectors with their coordinates */ + template + void setAllPoints(const std::vector& x, const std::vector& y, const std::vector& z) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + const auto N = x.size(); + m_points.resize(N); + for (size_t i = 0; i < N; i++) + m_points[i] = {static_cast(x[i]), static_cast(y[i]), static_cast(z[i])}; + m_minmax_valid = false; + wfWriteLock.unlock(); + markAllPointsAsNew(); + } + + /// \overload Prefer setAllPointsFast() instead + void setAllPoints(const std::vector& pts); + + /** Set the list of (X,Y,Z) point coordinates, DESTROYING the contents + * of the input vectors (via swap) */ + void setAllPointsFast(std::vector& pts) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + this->clear(); + m_points.swap(pts); + m_minmax_valid = false; + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); + } + + /** Get a const reference to the internal array of points */ + const std::vector& getArrayPoints() const + { + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + + return m_points; + } + + /** Empty the list of points. */ + void clear(); + + bool empty() const + { + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + return m_points.empty(); + } + + /** Adds a new point to the cloud */ + void insertPoint(float x, float y, float z); + + void insertPoint(const mrpt::math::TPoint3Df& p) + { + // lock already hold by insertPoint() below + insertPoint(p.x, p.y, p.z); + } + void insertPoint(const mrpt::math::TPoint3D& p) + { + // lock already hold by insertPoint() below + insertPoint(p.x, p.y, p.z); + } + + /** Read access to each individual point (checks for "i" in the valid + * range only in Debug). */ + const mrpt::math::TPoint3Df& operator[](size_t i) const + { +#ifdef _DEBUG + ASSERT_LT_(i, size()); +#endif + return m_points[i]; + } + + /// NOTE: This method is intentionally not protected by the shared_mutex, + /// since it's called in the inner loops of the octree, which acquires the + /// lock once. + const mrpt::math::TPoint3Df& getPoint3Df(size_t i) const { return m_points[i]; } + + /** Write an individual point (checks for "i" in the valid range only in + * Debug). */ + void setPoint(size_t i, const float x, const float y, const float z); + + /** Write an individual point (without checking validity of the index). + */ + void setPoint_fast(size_t i, const float x, const float y, const float z) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + m_points[i] = {x, y, z}; + m_minmax_valid = false; + wfWriteLock.unlock(); + markAllPointsAsNew(); + } + + /** Load the points from any other point map class supported by the + * adapter mrpt::viz::PointCloudAdapter. */ + template + void loadFromPointsMap(const POINTSMAP* themap); + // Must be implemented at the end of the header. + + /** Load the points from a list of mrpt::math::TPoint3D + */ + template + void loadFromPointsList(LISTOFPOINTS& pointsList) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + MRPT_START + const size_t N = pointsList.size(); + m_points.resize(N); + size_t idx; + typename LISTOFPOINTS::const_iterator it; + for (idx = 0, it = pointsList.begin(); idx < N; ++idx, ++it) + m_points[idx] = { + static_cast(it->x), static_cast(it->y), static_cast(it->z)}; + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); + MRPT_END + } + + /** @} */ + + /** @name Modify the appearance of the rendered points + @{ */ + void enableColorFromX(bool v = true) + { + m_colorFromDepth = v ? CPointCloud::colX : CPointCloud::colNone; + CVisualObject::notifyChange(); + } + void enableColorFromY(bool v = true) + { + m_colorFromDepth = v ? CPointCloud::colY : CPointCloud::colNone; + CVisualObject::notifyChange(); + } + void enableColorFromZ(bool v = true) + { + m_colorFromDepth = v ? CPointCloud::colZ : CPointCloud::colNone; + CVisualObject::notifyChange(); + } + + /** Sets the colors used as extremes when colorFromDepth is enabled. */ + void setGradientColors(const mrpt::img::TColorf& colorMin, const mrpt::img::TColorf& colorMax); + + /** @} */ + + /** Constructor */ + CPointCloud(); + + /** Private, virtual destructor: only can be deleted from smart pointers + */ + ~CPointCloud() override = default; + + void toYAMLMap(mrpt::containers::yaml& propertiesMap) const override; + + private: + /** Buffer for min/max coords when m_colorFromDepth is true. */ + mutable float m_min{0}, m_max{0}, m_max_m_min{0}, m_max_m_min_inv{0}; + /** Color linear function slope */ + mutable mrpt::img::TColorf m_col_slop, m_col_slop_inv; + mutable bool m_minmax_valid{false}; + + /** The colors used to interpolate when m_colorFromDepth is true. */ + mrpt::img::TColorf m_colorFromDepth_min = {0, 0, 0}, m_colorFromDepth_max = {0, 0, 1}; +}; + +/** Specialization mrpt::viz::PointCloudAdapter + * \ingroup mrpt_adapters_grp */ +template <> +class PointCloudAdapter +{ + private: + mrpt::viz::CPointCloud& m_obj; + + public: + /** The type of each point XYZ coordinates */ + using coords_t = float; + /** Has any color RGB info? */ + static constexpr bool HAS_RGB = false; + /** Has native RGB info (as floats)? */ + static constexpr bool HAS_RGBf = false; + /** Has native RGB info (as uint8_t)? */ + static constexpr bool HAS_RGBu8 = false; + + /** Constructor (accept a const ref for convenience) */ + PointCloudAdapter(const mrpt::viz::CPointCloud& obj) : + m_obj(*const_cast(&obj)) + { + } + /** Get number of points */ + size_t size() const { return m_obj.size(); } + /** Set number of points (to uninitialized values) */ + void resize(size_t N) { m_obj.resize(N); } + /** Does nothing as of now */ + void setDimensions(size_t height, size_t width) {} + /** Get XYZ coordinates of i'th point */ + template + void getPointXYZ(size_t idx, T& x, T& y, T& z) const + { + const auto& pt = m_obj[idx]; + x = pt.x; + y = pt.y; + z = pt.z; + } + /** Set XYZ coordinates of i'th point */ + void setPointXYZ(size_t idx, const coords_t x, const coords_t y, const coords_t z) + { + m_obj.setPoint_fast(idx, x, y, z); + } + + /** Set XYZ coordinates of i'th point */ + void setInvalidPoint(size_t idx) { m_obj.setPoint_fast(idx, 0, 0, 0); } + +}; // end of PointCloudAdapter + +// After declaring the adapter we can here implement this method: +template +void CPointCloud::loadFromPointsMap(const POINTSMAP* themap) +{ + CVisualObject::notifyChange(); + ASSERT_(themap != nullptr); + mrpt::viz::PointCloudAdapter pc_dst(*this); + const mrpt::viz::PointCloudAdapter pc_src(*themap); + const size_t N = pc_src.size(); + pc_dst.resize(N); + for (size_t i = 0; i < N; i++) + { + float x, y, z; + pc_src.getPointXYZ(i, x, y, z); + pc_dst.setPointXYZ(i, x, y, z); + } +} +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CPointCloudColoured.h b/libs/viz/include/mrpt/viz/CPointCloudColoured.h new file mode 100644 index 0000000000..16784497c7 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CPointCloudColoured.h @@ -0,0 +1,402 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include +#include +#include + +namespace mrpt::viz +{ +/** A cloud of points, each one with an individual colour (R,G,B). The alpha + * component is shared by all the points and is stored in the base member + * m_color_A. + * + * To load from a points-map, CPointCloudColoured::loadFromPointsMap(). + * + * This class uses smart optimizations while rendering to efficiently draw + * clouds of millions of points, using octrees. + * + * ![mrpt::viz::CPointCloudColoured](preview_CPointCloudColoured.png) + * + * \sa opengl::Scene, opengl::CPointCloud + * \ingroup mrpt_viz_grp + */ +class CPointCloudColoured : + virtual public CVisualObject, + public VisualObjectParams_Points, + public mrpt::viz::PLY_Importer, + public mrpt::viz::PLY_Exporter +{ + DEFINE_SERIALIZABLE(CPointCloudColoured, mrpt::viz) + + private: + /** Actually, an alias for the base class shader container of points. Kept + * to have an easy to use name. */ + std::vector& m_points = VisualObjectParams_Points::m_vertex_buffer_data; + + std::vector& m_point_colors = VisualObjectParams_Points::m_color_buffer_data; + + mutable size_t m_last_rendered_count{0}, m_last_rendered_count_ongoing{0}; + + public: + CPointCloudColoured() = default; + virtual ~CPointCloudColoured() override = default; + + void markAllPointsAsNew(); + + public: + auto internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf override; + + /** @name Read/Write of the list of points to render + @{ */ + + /** Inserts a new point into the point cloud. */ + void push_back(float x, float y, float z, float R, float G, float B, float A = 1); + + /** inserts a new point */ + void insertPoint(const mrpt::math::TPointXYZfRGBAu8& p); + + /** Set the number of points, with undefined contents */ + void resize(size_t N) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + m_points.resize(N); + m_point_colors.resize(N); + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); + } + + /** Like STL std::vector's reserve */ + void reserve(size_t N) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + m_points.reserve(N); + m_point_colors.reserve(N); + } + + /// NOTE: This method is intentionally not protected by the shared_mutex, + /// since it's called in the inner loops of the octree, which acquires the + /// lock once. + const mrpt::math::TPoint3Df& getPoint3Df(size_t i) const { return m_points[i]; } + + /** Write an individual point (checks for "i" in the valid range only in + * Debug). */ + void setPoint(size_t i, const mrpt::math::TPointXYZfRGBAu8& p); + + /** Like \a setPoint() but does not check for index out of bounds */ + void setPoint_fast(size_t i, const mrpt::math::TPointXYZfRGBAu8& p) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + m_points[i] = p.pt; + m_point_colors[i] = mrpt::img::TColor(p.r, p.g, p.b, p.a); + wfWriteLock.unlock(); + markAllPointsAsNew(); + } + + /** Like \a setPoint() but does not check for index out of bounds */ + void setPoint_fast(size_t i, const float x, const float y, const float z) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + m_points[i] = {x, y, z}; + wfWriteLock.unlock(); + markAllPointsAsNew(); + } + + /** Like \c setPointColor but without checking for out-of-index errors */ + void setPointColor_fast(size_t index, float R, float G, float B, float A = 1) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + m_point_colors[index].R = f2u8(R); + m_point_colors[index].G = f2u8(G); + m_point_colors[index].B = f2u8(B); + m_point_colors[index].A = f2u8(A); + } + void setPointColor_u8_fast(size_t index, uint8_t r, uint8_t g, uint8_t b, uint8_t a = 0xff) + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + m_point_colors[index].R = r; + m_point_colors[index].G = g; + m_point_colors[index].B = b; + m_point_colors[index].A = a; + } + /** Like \c getPointColor but without checking for out-of-index errors */ + void getPointColor_fast(size_t index, float& R, float& G, float& B) const + { + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + R = u8tof(m_point_colors[index].R); + G = u8tof(m_point_colors[index].G); + B = u8tof(m_point_colors[index].B); + } + void getPointColor_fast(size_t index, uint8_t& r, uint8_t& g, uint8_t& b) const + { + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + r = m_point_colors[index].R; + g = m_point_colors[index].B; + b = m_point_colors[index].B; + } + mrpt::img::TColor getPointColor(size_t index) const + { + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + return m_point_colors[index]; + } + + /** Return the number of points */ + size_t size() const + { + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + return m_points.size(); + } + /// Like size(), but without locking the data mutex (internal usage) + size_t size_unprotected() const { return m_points.size(); } + + bool empty() const + { + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + return m_points.empty(); + } + + /** Erase all the points */ + void clear() + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + m_points.clear(); + m_point_colors.clear(); + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); + } + + /** Load the points from any other point map class supported by the adapter + * mrpt::viz::PointCloudAdapter. */ + template + void loadFromPointsMap(const POINTSMAP* themap); + // Must be implemented at the end of the header. + + /** Get the number of elements actually rendered in the last render event. + */ + size_t getActuallyRendered() const { return m_last_rendered_count; } + /** @} */ + + /** @name Modify the appearance of the rendered points + @{ */ + + /** Regenerates the color of each point according the one coordinate + * (coord_index:0,1,2 for X,Y,Z) and the given color map. */ + void recolorizeByCoordinate( + const float coord_min, + const float coord_max, + const int coord_index = 2, + const mrpt::img::TColormap color_map = mrpt::img::cmJET); + + /** @} */ + + /** Render a subset of points (required by octree renderer) */ + void render_subset( + const bool all, const std::vector& idxs, const float render_area_sqpixels) const; + + void toYAMLMap(mrpt::containers::yaml& propertiesMap) const override; + + protected: + /** @name PLY Import virtual methods to implement in base classes + @{ */ + /** In a base class, reserve memory to prepare subsequent calls to + * PLY_import_set_vertex */ + void PLY_import_set_vertex_count(size_t N) override; + /** In a base class, reserve memory to prepare subsequent calls to + * PLY_import_set_face */ + void PLY_import_set_face_count([[maybe_unused]] size_t N) override {} + void PLY_import_set_vertex_timestamp( + [[maybe_unused]] size_t idx, [[maybe_unused]] const double unixTimestamp) override + { + // do nothing, this class ignores timestamps + } + /** In a base class, will be called after PLY_import_set_vertex_count() once + * for each loaded point. + * \param pt_color Will be nullptr if the loaded file does not provide + * color info. + */ + void PLY_import_set_vertex( + size_t idx, + const mrpt::math::TPoint3Df& pt, + const mrpt::img::TColorf* pt_color = nullptr) override; + /** @} */ + + /** @name PLY Export virtual methods to implement in base classes + @{ */ + size_t PLY_export_get_vertex_count() const override; + size_t PLY_export_get_face_count() const override { return 0; } + void PLY_export_get_vertex( + size_t idx, + mrpt::math::TPoint3Df& pt, + bool& pt_has_color, + mrpt::img::TColorf& pt_color) const override; + /** @} */ +}; + +/** Specialization + * mrpt::viz::PointCloudAdapter \ingroup + * mrpt_adapters_grp*/ +template <> +class PointCloudAdapter +{ + private: + mrpt::viz::CPointCloudColoured& m_obj; + + public: + /** The type of each point XYZ coordinates */ + using coords_t = float; + /** Has any color RGB info? */ + static constexpr bool HAS_RGB = true; + /** Has native RGB info (as floats)? */ + static constexpr bool HAS_RGBf = true; + /** Has native RGB info (as uint8_t)? */ + static constexpr bool HAS_RGBu8 = false; + + /** Constructor (accept a const ref for convenience) */ + PointCloudAdapter(const mrpt::viz::CPointCloudColoured& obj) : + m_obj(*const_cast(&obj)) + { + } + /** Get number of points */ + size_t size() const { return m_obj.size(); } + /** Set number of points (to uninitialized values) */ + void resize(size_t N) { m_obj.resize(N); } + /** Does nothing as of now */ + void setDimensions(size_t height, size_t width) {} + /** Get XYZ coordinates of i'th point */ + template + void getPointXYZ(size_t idx, T& x, T& y, T& z) const + { + const auto& p = m_obj.getPoint3Df(idx); + x = p.x; + y = p.y; + z = p.z; + } + /** Set XYZ coordinates of i'th point */ + void setPointXYZ(size_t idx, const coords_t x, const coords_t y, const coords_t z) + { + m_obj.setPoint_fast(idx, x, y, z); + } + + void setInvalidPoint(size_t idx) { m_obj.setPoint_fast(idx, 0, 0, 0); } + + /** Get XYZ_RGBf coordinates of i'th point */ + template + void getPointXYZ_RGBAf( + size_t idx, T& x, T& y, T& z, float& Rf, float& Gf, float& Bf, float& Af) const + { + const auto& pt = m_obj.getPoint3Df(idx); + const auto& col = m_obj.getPointColor(idx); + x = pt.x; + y = pt.y; + z = pt.z; + Rf = u8tof(col.R); + Gf = u8tof(col.G); + Bf = u8tof(col.B); + Af = u8tof(col.A); + } + /** Set XYZ_RGBf coordinates of i'th point */ + void setPointXYZ_RGBAf( + size_t idx, + const coords_t x, + const coords_t y, + const coords_t z, + const float Rf, + const float Gf, + const float Bf, + const float Af) + { + m_obj.setPoint( + idx, mrpt::math::TPointXYZfRGBAu8(x, y, z, f2u8(Rf), f2u8(Gf), f2u8(Bf), f2u8(Af))); + } + + /** Get XYZ_RGBu8 coordinates of i'th point */ + template + void getPointXYZ_RGBu8(size_t idx, T& x, T& y, T& z, uint8_t& r, uint8_t& g, uint8_t& b) const + { + const auto& pt = m_obj.getPoint3Df(idx); + const auto& col = m_obj.getPointColor(idx); + x = pt.x; + y = pt.y; + z = pt.z; + r = col.R; + g = col.G; + b = col.B; + } + /** Set XYZ_RGBu8 coordinates of i'th point */ + void setPointXYZ_RGBu8( + size_t idx, + const coords_t x, + const coords_t y, + const coords_t z, + const uint8_t r, + const uint8_t g, + const uint8_t b, + const uint8_t a = 0xff) + { + m_obj.setPoint_fast(idx, mrpt::math::TPointXYZfRGBAu8(x, y, z, r, g, b, a)); + } + + /** Get RGBf color of i'th point */ + void getPointRGBf(size_t idx, float& r, float& g, float& b) const + { + m_obj.getPointColor_fast(idx, r, g, b); + } + /** Set XYZ_RGBf coordinates of i'th point */ + void setPointRGBf(size_t idx, const float r, const float g, const float b) + { + m_obj.setPointColor_fast(idx, r, g, b); + } + + /** Get RGBu8 color of i'th point */ + void getPointRGBu8(size_t idx, uint8_t& r, uint8_t& g, uint8_t& b) const + { + m_obj.getPointColor_fast(idx, r, g, b); + } + /** Set RGBu8 coordinates of i'th point */ + void setPointRGBu8(size_t idx, const uint8_t r, const uint8_t g, const uint8_t b) + { + m_obj.setPointColor_u8_fast(idx, r, g, b); + } + +}; // end of PointCloudAdapter + +// After declaring the adapter we can here implement this method: +template +void CPointCloudColoured::loadFromPointsMap(const POINTSMAP* themap) +{ + CVisualObject::notifyChange(); + mrpt::viz::PointCloudAdapter pc_dst(*this); + const mrpt::viz::PointCloudAdapter pc_src(*themap); + const size_t N = pc_src.size(); + pc_dst.resize(N); + for (size_t i = 0; i < N; i++) + { + if constexpr (mrpt::viz::PointCloudAdapter::HAS_RGB) + { + float x, y, z, r, g, b, a; + pc_src.getPointXYZ_RGBAf(i, x, y, z, r, g, b, a); + pc_dst.setPointXYZ_RGBAf(i, x, y, z, r, g, b, a); + } + else + { + float x, y, z; + pc_src.getPointXYZ(i, x, y, z); + pc_dst.setPointXYZ_RGBAf(i, x, y, z, 0, 0, 0, 1); + } + } +} +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CPolyhedron.h b/libs/viz/include/mrpt/viz/CPolyhedron.h new file mode 100644 index 0000000000..5d1a999068 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CPolyhedron.h @@ -0,0 +1,928 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include + +namespace mrpt::viz +{ + +/** + * This class represents arbitrary polyhedra. The class includes a set of + * static methods to create common polyhedrons. The class includes many methods + * to create standard polyhedra, not intended to be fast but to be simple. For + * example, the dodecahedron is not created efficiently: first, an icosahedron + * is created, and a duality operator is applied to it, which yields the + * dodecahedron. This way, code is much smaller, although much slower. This is + * not a big problem, since polyhedron creation does not usually take a + * significant amount of time (they are created once and rendered many times). + * Polyhedra information and models have been gotten from the Wikipedia, + * https://wikipedia.org + * + * ![mrpt::viz::CPolyhedron](preview_CPolyhedron.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CPolyhedron : + virtual public CVisualObject, + public VisualObjectParams_Triangles, + public VisualObjectParams_Lines +{ + DEFINE_SERIALIZABLE(CPolyhedron, mrpt::viz) + public: + /** + * Struct used to store a polyhedron edge. The struct consists only of two + * vertex indices, used to access the polyhedron vertex list. + */ + struct TPolyhedronEdge + { + /** + * Vertices. + */ + uint32_t v1 = 0, v2 = 0; + /** + * Default constructor. Initializes to garbage. + */ + TPolyhedronEdge() = default; + /** + * Comparison against another edge. Simmetry is taken into account. + */ + bool operator==(const TPolyhedronEdge& e) const + { + if (e.v1 == v1 && e.v2 == v2) + return true; + else + return e.v1 == v2 && e.v2 == v1; + } + /** + * Given a set of vertices, computes the length of the vertex. + */ + double length(const std::vector& vs) const; + /** + * Destructor. + */ + ~TPolyhedronEdge() = default; + }; + /** + * Struct used to store a polyhedron face. Consists on a set of vertex + * indices and a normal vector. + */ + struct TPolyhedronFace + { + /** Vector of indices to the vertex list. */ + std::vector vertices; + /** Normal vector. */ + mrpt::math::TVector3D normal; + /** Fast default constructor. Initializes to garbage. */ + TPolyhedronFace() : vertices() {} + /** Destructor. */ + ~TPolyhedronFace() = default; + /** Given a set of vertices, computes the area of this face. */ + double area(const std::vector& vertices) const; + /** Given a set of vertices, get this face's center. */ + void getCenter( + const std::vector& vertices, mrpt::math::TPoint3D& p) const; + }; + + protected: + /** + * List of vertices presents in the polyhedron. + */ + std::vector m_Vertices; + /** + * List of polyhedron's edges. + */ + std::vector m_Edges; + /** + * List of polyhedron's faces. + */ + std::vector m_Faces; + /** + * This flag determines whether the polyhedron will be displayed as a solid + * object or as a set of edges. + */ + bool m_Wireframe{false}; + /** + * Mutable list of actual polygons, maintained for speed. + */ + mutable std::vector tempPolygons; + /** + * Whether the set of actual polygons is up to date or not. + */ + mutable bool polygonsUpToDate{false}; + + public: + /** Evaluates the bounding box of this object (including possible children) + * in the coordinate frame of the object parent. */ + [[nodiscard]] mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + // Static methods to create frequent polyhedra. More bizarre polyhedra are + // intended to be added in a near future. + + /** @name Platonic solids. + @{ + */ + /** + * Creates a regular tetrahedron (see + http://en.wikipedia.org/wiki/Tetrahedron). The tetrahedron is created as a + triangular pyramid whose edges and vertices are transitive. + * The tetrahedron is the dual to itself. +

+ * \sa + CreatePyramid,CreateJohnsonSolidWithConstantBase,CreateTruncatedTetrahedron + */ + static CPolyhedron::Ptr CreateTetrahedron(double radius); + /** + * Creates a regular cube, also called hexahedron (see + http://en.wikipedia.org/wiki/Hexahedron). The hexahedron is created as a + cubic prism which transitive edges. Another ways to create it include: +
  • Dual to an octahedron.
  • Parallelepiped with three + orthogonal, equally-lengthed vectors.
  • Triangular trapezohedron + with proper height.
+

+ * \sa + CreateOctahedron,getDual,CreateParallelepiped,CreateTrapezohedron,CreateTruncatedHexahedron,CreateTruncatedOctahedron,CreateCuboctahedron,CreateRhombicuboctahedron + */ + static CPolyhedron::Ptr CreateHexahedron(double radius); + /** + * Creates a regular octahedron (see + http://en.wikipedia.org/wiki/Octahedron). The octahedron is created as a + square bipyramid whit transitive edges and vertices. Another ways to + create an octahedron are: +
  • Dual to an hexahedron
  • Triangular antiprism with transitive + vertices.
  • Conveniently truncated tetrahedron.
+

+ * \sa + CreateHexahedron,getDual,CreateArchimedeanAntiprism,CreateTetrahedron,truncate,CreateTruncatedOctahedron,CreateTruncatedHexahedron,CreateCuboctahedron,CreateRhombicuboctahedron + */ + static CPolyhedron::Ptr CreateOctahedron(double radius); + /** + * Creates a regular dodecahedron (see + http://en.wikipedia.org/wiki/Dodecahedron). The dodecahedron is created as + the dual to an icosahedron. +

+ * \sa + CreateIcosahedron,getDual,CreateTruncatedDodecahedron,CreateTruncatedIcosahedron,CreateIcosidodecahedron,CreateRhombicosidodecahedron + */ + static CPolyhedron::Ptr CreateDodecahedron(double radius); + /** + * Creates a regular icosahedron (see + http://en.wikipedia.org/wiki/Icosahedron). The icosahedron is created as a + gyroelongated pentagonal bipyramid with transitive edges, and it's the + dual to a dodecahedron. +

+ * \sa + CreateJohnsonSolidWithConstantBase,CreateDodecahedron,getDual,CreateTruncatedIcosahedron,CreateTruncatedDodecahedron,CreateIcosidodecahedron,CreateRhombicosidodecahedron + */ + static CPolyhedron::Ptr CreateIcosahedron(double radius); + /** @} + */ + + /** @name Archimedean solids. + @{ + */ + /** + * Creates a truncated tetrahedron, consisting of four triangular faces and + for hexagonal ones (see + http://en.wikipedia.org/wiki/Truncated_tetrahedron). Its dual is the + triakis tetrahedron. +

+ * \sa CreateTetrahedron,CreateTriakisTetrahedron + */ + static CPolyhedron::Ptr CreateTruncatedTetrahedron(double radius); + /** + * Creates a cuboctahedron, consisting of six square faces and eight + triangular ones (see http://en.wikipedia.org/wiki/Cuboctahedron). There + are several ways to create a cuboctahedron: +
  • Hexahedron truncated to a certain extent.
  • Octahedron + truncated to a certain extent.
  • Cantellated + tetrahedron
  • Dual to a rhombic dodecahedron.
+

+ * \sa + CreateHexahedron,CreateOctahedron,truncate,CreateTetrahedron,cantellate,CreateRhombicuboctahedron,CreateRhombicDodecahedron, + */ + static CPolyhedron::Ptr CreateCuboctahedron(double radius); + /** + * Creates a truncated hexahedron, with six octogonal faces and eight + triangular ones (see http://en.wikipedia.org/wiki/Truncated_hexahedron). + The truncated octahedron is dual to the triakis octahedron. +

+ * \sa CreateHexahedron,CreateTriakisOctahedron + */ + static CPolyhedron::Ptr CreateTruncatedHexahedron(double radius); + /** + * Creates a truncated octahedron, with eight hexagons and eight squares + (see http://en.wikipedia.org/wiki/Truncated_octahedron). It's the dual to + the tetrakis hexahedron. +

+ * \sa CreateOctahedron,CreateTetrakisHexahedron + */ + static CPolyhedron::Ptr CreateTruncatedOctahedron(double radius); + /** + * Creates a rhombicuboctahedron, with 18 squares and 8 triangles (see + http://en.wikipedia.org/wiki/Rhombicuboctahedron), calculated as an + elongated square bicupola. It can also be calculated as a cantellated + hexahedron or octahedron, and its dual is the deltoidal icositetrahedron. + * If the second argument is set to false, the lower cupola is rotated, so + that the objet created is an elongated square gyrobicupola (see + http://en.wikipedia.org/wiki/Elongated_square_gyrobicupola). This is not + an archimedean solid, but a Johnson one, since it hasn't got vertex + transitivity. +

+ * \sa + CreateJohnsonSolidWithConstantBase,CreateHexahedron,CreateOctahedron,cantellate,CreateCuboctahedron,CreateDeltoidalIcositetrahedron + */ + static CPolyhedron::Ptr CreateRhombicuboctahedron(double radius, bool type = true); + /** + * Creates an icosidodecahedron, with 12 pentagons and 20 triangles (see + http://en.wikipedia.org/wiki/Icosidodecahedron). Certain truncations of + either a dodecahedron or an icosahedron yield an icosidodecahedron. + * The dual of the icosidodecahedron is the rhombic triacontahedron. + * If the second argument is set to false, the lower rotunda is rotated. In + this case, the object created is a pentagonal orthobirotunda (see + http://en.wikipedia.org/wiki/Pentagonal_orthobirotunda). This object + presents symmetry against the XY plane and is not vertex transitive, so + it's a Johnson's solid. +

+ * \sa + CreateDodecahedron,CreateIcosahedron,truncate,CreateRhombicosidodecahedron,CreateRhombicTriacontahedron + */ + static CPolyhedron::Ptr CreateIcosidodecahedron(double radius, bool type = true); + /** + * Creates a truncated dodecahedron, consisting of 12 dodecagons and 20 + triangles (see http://en.wikipedia.org/wiki/Truncated_dodecahedron). The + truncated dodecahedron is the dual to the triakis icosahedron. +

+ * \sa CreateDodecahedron,CreateTriakisIcosahedron + */ + static CPolyhedron::Ptr CreateTruncatedDodecahedron(double radius); + /** + * Creates a truncated icosahedron, consisting of 20 hexagons and 12 + pentagons. This object resembles a typical soccer ball (see + http://en.wikipedia.org/wiki/Truncated_icosahedron). The pentakis + dodecahedron is the dual to the truncated icosahedron. +

+ * \sa CreateIcosahedron,CreatePentakisDodecahedron + */ + static CPolyhedron::Ptr CreateTruncatedIcosahedron(double radius); + /** + * Creates a rhombicosidodecahedron, consisting of 30 squares, 12 pentagons + and 20 triangles (see + http://en.wikipedia.org/wiki/Rhombicosidodecahedron). This object can be + obtained as the cantellation of either a dodecahedron or an icosahedron. + The dual of the rhombicosidodecahedron is the deltoidal hexecontahedron. +

+ * \sa + CreateDodecahedron,CreateIcosahedron,CreateIcosidodecahedron,CreateDeltoidalHexecontahedron + */ + static CPolyhedron::Ptr CreateRhombicosidodecahedron(double radius); + /** @} + */ + + /** @name Other Johnson solids. + @{ + */ + /** + * Creates a pentagonal rotunda (half an icosidodecahedron), consisting of + * six pentagons, ten triangles and a decagon (see + * http://en.wikipedia.org/wiki/Pentagonal_rotunda). + * \sa CreateIcosidodecahedron,CreateJohnsonSolidWithConstantBase + */ + static CPolyhedron::Ptr CreatePentagonalRotunda(double radius); + /** @} + */ + + /** @name Catalan solids. + @{ + */ + /** + * Creates a triakis tetrahedron, dual to the truncated tetrahedron. This + body consists of 12 isosceles triangles (see + http://en.wikipedia.org/wiki/Triakis_tetrahedron). +

+ * \sa CreateTruncatedTetrahedron + */ + static CPolyhedron::Ptr CreateTriakisTetrahedron(double radius); + + /** + * Creates a rhombic dodecahedron, dual to the cuboctahedron. This body + consists of 12 rhombi (see + http://en.wikipedia.org/wiki/Rhombic_dodecahedron). +

+ * \sa CreateCuboctahedron + */ + static CPolyhedron::Ptr CreateRhombicDodecahedron(double radius); + + /** + * Creates a triakis octahedron, dual to the truncated hexahedron. This + body consists of 24 isosceles triangles (see + http://en.wikipedia.org/wiki/Triakis_octahedron). +

+ * \sa CreateTruncatedHexahedron + */ + static CPolyhedron::Ptr CreateTriakisOctahedron(double radius); + + /** + * Creates a tetrakis hexahedron, dual to the truncated octahedron. This + body consists of 24 isosceles triangles (see + http://en.wikipedia.org/wiki/Tetrakis_hexahedron). +

+ * \sa CreateTruncatedOctahedron + */ + static CPolyhedron::Ptr CreateTetrakisHexahedron(double radius); + + /** + * Creates a deltoidal icositetrahedron, dual to the rhombicuboctahedron. + This body consists of 24 kites (see + http://en.wikipedia.org/wiki/Deltoidal_icositetrahedron). +

+ * \sa CreateRhombicuboctahedron + */ + static CPolyhedron::Ptr CreateDeltoidalIcositetrahedron(double radius); + + /** + * Creates a rhombic triacontahedron, dual to the icosidodecahedron. This + body consists of 30 rhombi (see + http://en.wikipedia.org/wiki/Rhombic_triacontahedron). +

+ * \sa CreateIcosidodecahedron + */ + static CPolyhedron::Ptr CreateRhombicTriacontahedron(double radius); + + /** + * Creates a triakis icosahedron, dual to the truncated dodecahedron. This + body consists of 60 isosceles triangles + http://en.wikipedia.org/wiki/Triakis_icosahedron). +

+ * \sa CreateTruncatedDodecahedron + */ + static CPolyhedron::Ptr CreateTriakisIcosahedron(double radius); + + /** + * Creates a pentakis dodecahedron, dual to the truncated icosahedron. This + body consists of 60 isosceles triangles (see + http://en.wikipedia.org/wiki/Pentakis_dodecahedron). +

+ * \sa CreateTruncatedIcosahedron + */ + static CPolyhedron::Ptr CreatePentakisDodecahedron(double radius); + + /** + * Creates a deltoidal hexecontahedron, dual to the rhombicosidodecahedron. + This body consists of 60 kites (see + http://en.wikipedia.org/wiki/Deltoidal_hexecontahedron). +

+ * \sa CreateRhombicosidodecahedron + */ + static CPolyhedron::Ptr CreateDeltoidalHexecontahedron(double radius); + /** @} + */ + + /** @name Customizable polyhedra + @{ + */ + /** + * Creates a cubic prism, given the coordinates of two opposite vertices. + * Each edge will be parallel to one of the coordinate axes, although the + * orientation may change by assigning a pose to the object. + * \sa CreateCubicPrism(const mrpt::math::TPoint3D &,const + * mrpt::math::TPoint3D + * &),CreateParallelepiped,CreateCustomPrism,CreateRegularPrism,CreateArchimedeanRegularPrism + */ + static CPolyhedron::Ptr CreateCubicPrism( + double x1, double x2, double y1, double y2, double z1, double z2); + /** + * Creates a cubic prism, given two opposite vertices. + * \sa + * CreateCubicPrism(double,double,double,double,double,double),CreateParallelepiped,CreateCustomPrism,CreateRegularPrism,CreateArchimedeanRegularPrism + */ + static CPolyhedron::Ptr CreateCubicPrism( + const mrpt::math::TPoint3D& p1, const mrpt::math::TPoint3D& p2); + /** + * Creates a custom pyramid, using a set of 2D vertices which will lie on + * the XY plane. + * \sa + * CreateDoublePyramid,CreateFrustum,CreateBifrustum,CreateRegularPyramid + */ + static CPolyhedron::Ptr CreatePyramid( + const std::vector& baseVertices, double height); + /** + * Creates a double pyramid, using a set of 2D vertices which will lie on + * the XY plane. The second height is used with the downwards pointing + * pyramid, so that it will effectively point downwards if it's positive. + * \sa CreatePyramid,CreateBifrustum,CreateRegularDoublePyramid + */ + static CPolyhedron::Ptr CreateDoublePyramid( + const std::vector& baseVertices, double height1, double height2); + /** + * Creates a truncated pyramid, using a set of vertices which will lie on + * the XY plane. + * Do not try to use with a ratio equal to zero; use CreatePyramid instead. + * When using a ratio of 1, it will create a Prism. + * \sa CreatePyramid,CreateBifrustum + */ + static CPolyhedron::Ptr CreateTruncatedPyramid( + const std::vector& baseVertices, double height, double ratio); + /** + * This is a synonym for CreateTruncatedPyramid. + * \sa CreateTruncatedPyramid + */ + static CPolyhedron::Ptr CreateFrustum( + const std::vector& baseVertices, double height, double ratio); + /** + * Creates a custom prism with vertical edges, given any base which will + * lie on the XY plane. + * \sa + * CreateCubicPrism,CreateCustomAntiprism,CreateRegularPrism,CreateArchimedeanRegularPrism + */ + static CPolyhedron::Ptr CreateCustomPrism( + const std::vector& baseVertices, double height); + /** + * Creates a custom antiprism, using two custom bases. For better results, + * the top base should be slightly rotated with respect to the bottom one. + * \sa + * CreateCustomPrism,CreateRegularAntiprism,CreateArchimedeanRegularAntiprism + */ + static CPolyhedron::Ptr CreateCustomAntiprism( + const std::vector& bottomBase, + const std::vector& topBase, + double height); + /** + * Creates a parallelepiped, given a base point and three vectors + * represented as points. + * \sa CreateCubicPrism + */ + static CPolyhedron::Ptr CreateParallelepiped( + const mrpt::math::TPoint3D& base, + const mrpt::math::TPoint3D& v1, + const mrpt::math::TPoint3D& v2, + const mrpt::math::TPoint3D& v3); + /** + * Creates a bifrustum, or double truncated pyramid, given a base which + * will lie on the XY plane. + * \sa CreateFrustum,CreateDoublePyramid + */ + static CPolyhedron::Ptr CreateBifrustum( + const std::vector& baseVertices, + double height1, + double ratio1, + double height2, + double ratio2); + /** + * Creates a trapezohedron, consisting of 2*N kites, where N is the number + * of edges in the base. The base radius controls the polyhedron height, + * whilst the distance between bases affects the height. + * When the number of edges equals 3, the polyhedron is actually a + * parallelepiped, and it can even be a cube. + */ + static CPolyhedron::Ptr CreateTrapezohedron( + uint32_t numBaseEdges, double baseRadius, double basesDistance); + /** + * Creates an antiprism whose base is a regular polygon. The upper base is + * rotated \f$\frac\pi N\f$ with respect to the lower one, where N is the + * number of vertices in the base, and thus the lateral triangles are + * isosceles. + * \sa CreateCustomAntiprism,CreateArchimedeanRegularAntiprism + */ + static CPolyhedron::Ptr CreateRegularAntiprism( + uint32_t numBaseEdges, double baseRadius, double height); + /** + * Creates a regular prism whose base is a regular polygon and whose edges + * are either parallel or perpendicular to the XY plane. + * \sa CreateCubicPrism,CreateCustomPrism,CreateArchimedeanRegularAntiprism + */ + static CPolyhedron::Ptr CreateRegularPrism( + uint32_t numBaseEdges, double baseRadius, double height); + /** + * Creates a regular pyramid whose base is a regular polygon. + * \sa CreatePyramid + */ + static CPolyhedron::Ptr CreateRegularPyramid( + uint32_t numBaseEdges, double baseRadius, double height); + /** + * Creates a regular double pyramid whose base is a regular polygon. + * \sa CreateDoublePyramid + */ + static CPolyhedron::Ptr CreateRegularDoublePyramid( + uint32_t numBaseEdges, double baseRadius, double height1, double height2); + /** + * Creates a regular prism whose lateral area is comprised of squares, and + * so each face of its is a regular polygon. Due to vertex transitivity, the + * resulting object is always archimedean. + * \sa CreateRegularPrism,CreateCustomPrism + */ + static CPolyhedron::Ptr CreateArchimedeanRegularPrism(uint32_t numBaseEdges, double baseRadius); + /** + * Creates a regular antiprism whose lateral polygons are equilateral + * triangles, and so each face of its is a regular polygon. Due to vertex + * transitivity, the resulting object is always archimedean. + * \sa CreateRegularAntiprism,CreateCustomAntiprism + */ + static CPolyhedron::Ptr CreateArchimedeanRegularAntiprism( + uint32_t numBaseEdges, double baseRadius); + /** + * Creates a regular truncated pyramid whose base is a regular polygon. + * \sa CreateTruncatedPyramid + */ + static CPolyhedron::Ptr CreateRegularTruncatedPyramid( + uint32_t numBaseEdges, double baseRadius, double height, double ratio); + /** + * This is a synonym for CreateRegularTruncatedPyramid. + * \sa CreateRegularTruncatedPyramid + */ + static CPolyhedron::Ptr CreateRegularFrustum( + uint32_t numBaseEdges, double baseRadius, double height, double ratio); + /** + * Creates a bifrustum (double truncated pyramid) whose base is a regular + * polygon lying in the XY plane. + * \sa CreateBifrustum + */ + static CPolyhedron::Ptr CreateRegularBifrustum( + uint32_t numBaseEdges, + double baseRadius, + double height1, + double ratio1, + double height2, + double ratio2); + /** + * Creates a cupola. + * \throw std::logic_error if the number of edges is odd or less than four. + */ + static CPolyhedron::Ptr CreateCupola(uint32_t numBaseEdges, double edgeLength); + /** + * Creates a trapezohedron whose dual is exactly an archimedean antiprism. + * Creates a cube if numBaseEdges is equal to 3. + * \todo Actually resulting height is significantly higher than that passed + * to the algorithm. + * \sa CreateTrapezohedron,CreateArchimedeanRegularAntiprism,getDual + */ + static CPolyhedron::Ptr CreateCatalanTrapezohedron(uint32_t numBaseEdges, double height); + /** + * Creates a double pyramid whose dual is exactly an archimedean prism. + * Creates an octahedron if numBaseEdges is equal to 4. + * \todo Actually resulting height is significantly higher than that passed + * to the algorithm. + * \sa CreateDoublePyramid,CreateArchimedeanRegularPrism,getDual + */ + static CPolyhedron::Ptr CreateCatalanDoublePyramid(uint32_t numBaseEdges, double height); + /** + * Creates a series of concatenated solids (most of which are prismatoids) + whose base is a regular polygon with a given number of edges. Every face + of the resulting body will be a regular polygon, so it is a Johnson solid; + in special cases, it may be archimedean or even platonic. + * The shape of the body is defined by the string argument, which can + include one or more of the following: +
+ + + + + + + + + + + + + +
StringBodyRestrictions
P+Upward pointing pyramidMust be the last + object, vertex number cannot surpass 5
P-Downward pointing pyramidMust be the first + object, vertex number cannot surpass 5
C+Upward pointing cupolaMust be the last object, + vertex number must be an even number in the range 4-10.
C-Downward pointing cupolaMust be the first + object, vertex number must be an even number in the range 4-10.
GC+Upward pointing cupola, rotatedMust be the + last object, vertex number must be an even number in the range + 4-10.
GC-Downward pointing cupola, rotatedMust be the + first object, vertex number must be an even number in the range + 4-10.
PRArchimedean prismCannot abut other + prism
AArchimedean antiprismNone
R+Upward pointing rotundaMust be the last + object, vertex number must be exactly 10
R-Downward pointing rotundaMust be the first + object, vertex number must be exactly 10
GR+Upward pointing rotunda, rotatedMust be the + last object, vertex number must be exactly 10
GR-Downward pointing rotundaMust be the first + object, vertex number must be exactly 10
+ * Some examples of bodies are: +
+ + + + + + + + +
StringVerticesResulting + body
P+3Tetrahedron
PR4Hexahedron
P-P+4Octahedron
A3Octahedron
C+PRC-8Rhombicuboctahedron
P-AP+5Icosahedron
R-R+10Icosidodecahedron
+ */ + static CPolyhedron::Ptr CreateJohnsonSolidWithConstantBase( + uint32_t numBaseEdges, double baseRadius, const std::string& components, size_t shifts = 0); + /** @} + */ + + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + + /** + * Gets a list with the polyhedron's vertices. + */ + void getVertices(std::vector& vertices) const { vertices = m_Vertices; } + /** + * Gets a list with the polyhedron's edges. + */ + void getEdges(std::vector& edges) const { edges = m_Edges; } + /** + * Gets a list with the polyhedron's faces. + */ + void getFaces(std::vector& faces) const { faces = m_Faces; } + /** + * Gets the amount of vertices. + */ + uint32_t getNumberOfVertices() const { return m_Vertices.size(); } + /** + * Gets the amount of edges. + */ + uint32_t getNumberOfEdges() const { return m_Edges.size(); } + /** + * Gets the amount of faces. + */ + uint32_t getNumberOfFaces() const { return m_Faces.size(); } + /** + * Gets a vector with each edge's length. + */ + void getEdgesLength(std::vector& lengths) const; + /** + * Gets a vector with each face's area. Won't work properly if the polygons + * are not convex. + */ + void getFacesArea(std::vector& areas) const; + /** + * Gets the polyhedron volume. Won't work properly if the polyhedron is not + * convex. + */ + double getVolume() const; + /** + * Returns whether the polyhedron will be rendered as a wireframe object. + */ + bool isWireframe() const { return m_Wireframe; } + /** + * Sets whether the polyhedron will be rendered as a wireframe object. + */ + void setWireframe(bool enabled = true) + { + m_Wireframe = enabled; + CVisualObject::notifyChange(); + } + /** + * Gets the polyhedron as a set of polygons. + * \sa mrpt::math::TPolygon3D + */ + void getSetOfPolygons(std::vector& vec) const; + /** + * Gets the polyhedron as a set of polygons, with the pose transformation + * already applied. + * \sa mrpt::math::TPolygon3D,mrpt::poses::CPose3D + */ + void getSetOfPolygonsAbsolute(std::vector& vec) const; + /** Gets the intersection of two polyhedra, either as a set or as a matrix + * of intersections. Each intersection is represented by a TObject3D. + * \sa mrpt::math::TObject3D + */ + template + static size_t getIntersection( + const CPolyhedron::Ptr& p1, const CPolyhedron::Ptr& p2, T& container); + /** + * Returns true if the polygon is a completely closed object. + */ + bool isClosed() const + { + for (size_t i = 0; i < m_Vertices.size(); i++) + if (edgesInVertex(i) != facesInVertex(i)) return false; + return true; + } + /** + * Recomputes polygons, if necessary, so that each one is convex. + */ + void makeConvexPolygons(); + /** + * Gets the center of the polyhedron. + */ + void getCenter(mrpt::math::TPoint3D& center) const; + /** + * Creates a random polyhedron from the static methods. + */ + static CPolyhedron::Ptr CreateRandomPolyhedron(double radius); + + /** @name Polyhedron special operations. + @{ + */ + /** + * Given a polyhedron, creates its dual. + * \sa truncate,cantellate,augment + * \throw std::logic_error Can't get the dual to this polyhedron. + */ + CPolyhedron::Ptr getDual() const; + /** + * Truncates a polyhedron to a given factor. + * \sa getDual,cantellate,augment + * \throw std::logic_error Polyhedron truncation results in skew polygons + * and thus it's impossible to perform. + */ + CPolyhedron::Ptr truncate(double factor) const; + /** + * Cantellates a polyhedron to a given factor. + * \sa getDual,truncate,augment + */ + CPolyhedron::Ptr cantellate(double factor) const; + /** + * Augments a polyhedron to a given height. This operation is roughly dual + * to the truncation: given a body P, the operation dtdP and aP yield + * resembling results. + * \sa getDual,truncate,cantellate + */ + CPolyhedron::Ptr augment(double height) const; + /** + * Augments a polyhedron to a given height. This method only affects to + * faces with certain number of vertices. + * \sa augment(double) const + */ + CPolyhedron::Ptr augment(double height, size_t numVertices) const; + /** + * Augments a polyhedron, so that the resulting triangles are equilateral. + * If the argument is true, triangles are "cut" from the polyhedron, instead + * of being added. + * \throw std::logic_error a non-regular face has been found. + * \sa augment(double) const + */ + CPolyhedron::Ptr augment(bool direction = false) const; + /** + * Augments a polyhedron, so that the resulting triangles are equilateral; + * affects only faces with certain number of faces. If the second argument + * is true, triangles are "cut" from the polyhedron. + * \throw std::logic_error a non-regular face has been found. + * \sa augment(double) const + */ + CPolyhedron::Ptr augment(size_t numVertices, bool direction = false) const; + /** + * Rotates a polyhedron around the Z axis a given amount of radians. In + *some cases, this operation may be necessary to view the symmetry between + *related objects. + * \sa scale + */ + CPolyhedron::Ptr rotate(double angle) const; + /** + * Scales a polyhedron to a given factor. + * \throw std::logic_error factor is not a strictly positive number. + * \sa rotate + */ + CPolyhedron::Ptr scale(double factor) const; + /** @} + */ + /** + * Updates the mutable list of polygons used in rendering and ray tracing. + */ + void updatePolygons() const; + + private: + /** + * Generates a list of 2D vertices constituting a regular polygon. + */ + static std::vector generateBase(uint32_t numBaseEdges, double baseRadius); + /** + * Generates a list of 2D vertices constituting a regular polygon, with an + * angle shift which makes it suitable for antiprisms. + */ + static std::vector generateShiftedBase( + uint32_t numBaseEdges, double baseRadius); + /** + * Generates a list of 3D vertices constituting a regular polygon, + * appending it to an existing vector. + */ + static void generateBase( + uint32_t numBaseEdges, + double baseRadius, + double height, + std::vector& vec); + /** + * Generates a list of 3D vertices constituting a regular polygon + * conveniently shifted, appending it to an existing vector. + */ + static void generateShiftedBase( + uint32_t numBaseEdges, + double baseRadius, + double height, + double shift, + std::vector& vec); + /** + * Calculates the normal vector to a face. + */ + bool setNormal(TPolyhedronFace& f, bool doCheck = true); + /** + * Adds, to the existing list of edges, each edge in a given face. + */ + void addEdges(const TPolyhedronFace& e); + /** + * Checks whether a set of faces is suitable for a set of vertices. + */ + static bool checkConsistence( + const std::vector& vertices, const std::vector& faces); + /** + * Returns how many edges converge in a given vertex. + */ + size_t edgesInVertex(size_t vertex) const; + /** + * Returns how many faces converge in a given vertex. + */ + size_t facesInVertex(size_t vertex) const; + + public: + /** + * Basic empty constructor. + */ + CPolyhedron() : m_Vertices(), m_Edges(), m_Faces() {} + /** + * Basic constructor with a list of vertices and another of faces, checking + * for correctness. + */ + CPolyhedron( + const std::vector& vertices, + const std::vector& faces, + bool doCheck = true) : + m_Vertices(vertices), m_Edges(), m_Faces(faces), m_Wireframe(false), polygonsUpToDate(false) + { + InitFromVertAndFaces(vertices, faces, doCheck); + } + void InitFromVertAndFaces( + const std::vector& vertices, + const std::vector& faces, + bool doCheck = true) + { + if (doCheck && !checkConsistence(vertices, faces)) + throw std::logic_error("Face list accesses a vertex out of range"); + for (auto& mFace : m_Faces) + { + if (!setNormal(mFace, doCheck)) throw std::logic_error("Bad face specification"); + addEdges(mFace); + } + } + + CPolyhedron(const std::vector& polys); + + CPolyhedron( + const std::vector& vertices, + const std::vector>& faces); + + /** Creates a polyhedron without checking its correctness. */ + static CPolyhedron::Ptr CreateNoCheck( + const std::vector& vertices, const std::vector& faces); + /** Creates an empty Polyhedron. */ + static CPolyhedron::Ptr CreateEmpty(); + /** Destructor. */ + ~CPolyhedron() override = default; +}; + +// Implemented after the definition of Smart::Ptrs in the _POST() macro above. +template +size_t CPolyhedron::getIntersection( + const CPolyhedron::Ptr& p1, const CPolyhedron::Ptr& p2, T& container) +{ + std::vector polys1, polys2; + p1->getSetOfPolygonsAbsolute(polys1); + p2->getSetOfPolygonsAbsolute(polys2); + return mrpt::math::intersect(polys1, polys2, container); +} + +/** + * Reads a polyhedron edge from a binary stream. + */ +mrpt::serialization::CArchive& operator>>( + mrpt::serialization::CArchive& in, CPolyhedron::TPolyhedronEdge& o); +/** + * Writes a polyhedron edge to a binary stream. + */ +mrpt::serialization::CArchive& operator<<( + mrpt::serialization::CArchive& out, const CPolyhedron::TPolyhedronEdge& o); +/** + * Reads a polyhedron face from a binary stream. + */ +mrpt::serialization::CArchive& operator>>( + mrpt::serialization::CArchive& in, CPolyhedron::TPolyhedronFace& o); +/** + * Writes a polyhedron face to a binary stream. + */ +mrpt::serialization::CArchive& operator<<( + mrpt::serialization::CArchive& out, const CPolyhedron::TPolyhedronFace& o); +} // namespace mrpt::viz + +namespace mrpt::typemeta +{ +// Specialization must occur in the same namespace +MRPT_DECLARE_TTYPENAME_NAMESPACE(CPolyhedron::TPolyhedronEdge, mrpt::viz) +MRPT_DECLARE_TTYPENAME_NAMESPACE(CPolyhedron::TPolyhedronFace, mrpt::viz) +} // namespace mrpt::typemeta diff --git a/libs/viz/include/mrpt/viz/CSetOfLines.h b/libs/viz/include/mrpt/viz/CSetOfLines.h new file mode 100644 index 0000000000..af0fcd5918 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CSetOfLines.h @@ -0,0 +1,219 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include + +namespace mrpt::viz +{ +/** A set of independent lines (or segments), one line with its own start and + * end positions (X,Y,Z). Optionally, the vertices can be also shown as dots. + * + * ![mrpt::viz::CSetOfLines](preview_CSetOfLines.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CSetOfLines : + virtual public CVisualObject, + public VisualObjectParams_Points, + public VisualObjectParams_Lines +{ + DEFINE_SERIALIZABLE(CSetOfLines, mrpt::viz) + protected: + std::vector m_Segments; + + public: + /** Clear the list of segments */ + void clear() + { + m_Segments.clear(); + CVisualObject::notifyChange(); + } + + [[nodiscard]] float getVerticesPointSize() const + { + return VisualObjectParams_Points::getPointSize(); + } + + /** Enable showing vertices as dots if size_points>0 */ + void setVerticesPointSize(const float size_points) + { + VisualObjectParams_Points::setPointSize(size_points); + } + + /** Appends a line to the set. */ + void appendLine(const mrpt::math::TSegment3D& sgm) + { + m_Segments.push_back(sgm); + CVisualObject::notifyChange(); + } + + /** Appends a line to the set, given the coordinates of its bounds. */ + void appendLine(double x0, double y0, double z0, double x1, double y1, double z1) + { + appendLine( + mrpt::math::TSegment3D(mrpt::math::TPoint3D(x0, y0, z0), mrpt::math::TPoint3D(x1, y1, z1))); + CVisualObject::notifyChange(); + } + + /** Appends a line whose starting point is the end point of the last line + * (similar to OpenGL's GL_LINE_STRIP) + * \exception std::exception If there is no previous segment */ + void appendLineStrip(float x, float y, float z) + { + ASSERT_(!this->empty()); + this->appendLine(this->rbegin()->point2, mrpt::math::TPoint3D(x, y, z)); + } + + //! \overload + template + void appendLineStrip(const U& point) + { + ASSERT_(!this->empty()); + this->appendLine(this->rbegin()->point2, point); + } + + /** + * Appends any iterable collection of lines to the set. Note that this + * includes another CSetOfLines. + * \sa appendLine + */ + template + void appendLines(const T& sgms) + { + m_Segments.insert(m_Segments.end(), sgms.begin(), sgms.end()); + CVisualObject::notifyChange(); + } + /** + * Appends certain amount of lines, located between two iterators, into the + * set. + * \sa appendLine + */ + template + void appendLines(const T_it& begin, const T_it& end) + { + m_Segments.reserve(m_Segments.size() + (end - begin)); + m_Segments.insert(m_Segments.end(), begin, end); + CVisualObject::notifyChange(); + } + /** + * Resizes the set. + * \sa reserve + */ + void resize(size_t nLines) + { + m_Segments.resize(nLines); + CVisualObject::notifyChange(); + } + /** + * Reserves an amount of lines to the set. This method should be used when + * some known amount of lines is going to be inserted, so that only a memory + * allocation is needed. + * \sa resize + */ + void reserve(size_t r) + { + m_Segments.reserve(r); + CVisualObject::notifyChange(); + } + /** + * Inserts a line, given its bounds. Works with any pair of objects with + * access to x, y and z members. + */ + template + void appendLine(T p0, U p1) + { + appendLine(p0.x, p0.y, p0.z, p1.x, p1.y, p1.z); + CVisualObject::notifyChange(); + } + + /** Returns the total count of lines in this set. */ + [[nodiscard]] size_t size() const { return m_Segments.size(); } + + /** Returns true if there are no line segments. */ + [[nodiscard]] bool empty() const { return m_Segments.empty(); } + + /** Sets a specific line in the set, given its index. \sa appendLine */ + void setLineByIndex(size_t index, const mrpt::math::TSegment3D& segm); + + /** Sets a specific line in the set, given its index. \sa appendLine */ + void setLineByIndex( + size_t index, double x0, double y0, double z0, double x1, double y1, double z1) + { + setLineByIndex( + index, + mrpt::math::TSegment3D(mrpt::math::TPoint3D(x0, y0, z0), mrpt::math::TPoint3D(x1, y1, z1))); + CVisualObject::notifyChange(); + } + + /** Gets a specific line in the set, given its index. \sa getLineByIndex */ + void getLineByIndex( + size_t index, double& x0, double& y0, double& z0, double& x1, double& y1, double& z1) const; + + // Iterator management + using iterator = std::vector::iterator; + using reverse_iterator = std::vector::reverse_iterator; + using const_iterator = std::vector::const_iterator; + using const_reverse_iterator = std::vector::const_reverse_iterator; + /** + * Beginning const iterator. + * \sa end,rbegin,rend + */ + [[nodiscard]] const_iterator begin() const { return m_Segments.begin(); } + [[nodiscard]] iterator begin() + { + CVisualObject::notifyChange(); + return m_Segments.begin(); + } + /** + * Ending const iterator. + * \sa begin,rend,rbegin + */ + [[nodiscard]] const_iterator end() const { return m_Segments.end(); } + [[nodiscard]] iterator end() + { + CVisualObject::notifyChange(); + return m_Segments.end(); + } + /** + * Beginning const reverse iterator (actually, accesses the end of the + * set). + * \sa rend,begin,end + */ + [[nodiscard]] const_reverse_iterator rbegin() const { return m_Segments.rbegin(); } + /** + * Ending const reverse iterator (actually, refers to the starting point of + * the set). + * \sa rbegin,end,begin + */ + [[nodiscard]] const_reverse_iterator rend() const { return m_Segments.rend(); } + + /** Evaluates the bounding box of this object (including possible children) + * in the coordinate frame of the object parent. */ + [[nodiscard]] mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Constructor */ + CSetOfLines(); + /** Constructor with a initial set of lines. */ + CSetOfLines(const std::vector& sgms, bool antiAliasing = true); + /** Private, virtual destructor: only can be deleted from smart pointers. */ + ~CSetOfLines() override = default; +}; +/** Inserts a set of segments into the list. Allows call chaining. + * \sa mrpt::viz::CSetOfLines::appendLines + */ +template +CSetOfLines::Ptr& operator<<(CSetOfLines::Ptr& l, const T& s) +{ + l->appendLines(s.begin(), s.end()); + return l; +} +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CSetOfObjects.h b/libs/viz/include/mrpt/viz/CSetOfObjects.h new file mode 100644 index 0000000000..e69a4e4d7d --- /dev/null +++ b/libs/viz/include/mrpt/viz/CSetOfObjects.h @@ -0,0 +1,184 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include // All these are needed for the auxiliary methods posePDF2opengl() +#include + +namespace mrpt::viz +{ +/** A set of object, which are referenced to the coordinates framework + * established in this object. + * It can be established a hierarchy of "CSetOfObjects", where the coordinates + * framework of each one will be referenced to the parent's one. + * The list of child objects is accessed directly as in the class Scene + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CSetOfObjects : public CVisualObject +{ + DEFINE_SERIALIZABLE(CSetOfObjects, mrpt::viz) + + protected: + /** The list of child objects. + * Objects are automatically deleted when calling "clear" or in the + * destructor. + */ + ListVisualObjects m_objects; + + public: + CSetOfObjects() = default; + virtual ~CSetOfObjects() override = default; + + using const_iterator = ListVisualObjects::const_iterator; + using iterator = ListVisualObjects::iterator; + + const_iterator begin() const { return m_objects.begin(); } + const_iterator end() const { return m_objects.end(); } + iterator begin() { return m_objects.begin(); } + iterator end() { return m_objects.end(); } + /** Inserts a set of objects into the list. + */ + template + void insertCollection(const T& objs) + { + insert(objs.begin(), objs.end()); + } + /** Insert a new object to the list. + */ + void insert(const CVisualObject::Ptr& newObject); + + /** Inserts a set of objects, bounded by iterators, into the list. + */ + template + void insert(const T_it& begin, const T_it& end) + { + for (T_it it = begin; it != end; it++) insert(*it); + } + + /** Clear the list of objects in the scene, deleting objects' memory. + */ + void clear(); + + /** Returns number of objects. */ + size_t size() { return m_objects.size(); } + /** Returns true if there are no objects. */ + bool empty() const { return m_objects.empty(); } + + /** Returns the first object with a given name, or a nullptr pointer if not + * found. + */ + CVisualObject::Ptr getByName(const std::string& str); + + /** Returns the i'th object of a given class (or of a descendant class), or + nullptr (an empty smart pointer) if not found. + * Example: + * \code + CSphere::Ptr obs = myscene.getByClass(); + * \endcode + * By default (ith=0), the first observation is returned. + */ + template + typename T::Ptr getByClass(size_t ith = 0) const; + + /** Removes the given object from the scene (it also deletes the object to + * free its memory). + */ + void removeObject(const CVisualObject::Ptr& obj); + + /** Retrieves a list of all objects in text form + * \deprecated Prefer asYAML() (since MRPT 2.1.3) */ + void dumpListOfObjects(std::vector& lst) const; + + /** Prints all objects in human-readable YAML form. + * \note (New in MRPT 2.1.3) */ + mrpt::containers::yaml asYAML() const; + + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + + CVisualObject& setColor_u8(const mrpt::img::TColor& c) override; + CVisualObject& setColorA_u8(const uint8_t a) override; + bool contains(const CVisualObject::Ptr& obj) const; + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** @name pose_pdf -> 3d objects auxiliary templates + @{ */ + // The reason this code is here is to exploit C++'s "T::template function()" + // in order to + // define the members getAs3DObject() in several classes in mrpt-base with + // its argument + // being a class (CSetOfObjects) which is actually declared here, in + // mrpt-opengl. + // Implementations are in "pose_pdfs.cpp", not in "CSetOfObjects" (historic + // reasons...) + + /** Returns a representation of a the PDF - this is just an auxiliary + * function, it's more natural to call + * mrpt::poses::CPosePDF::getAs3DObject */ + static CSetOfObjects::Ptr posePDF2opengl(const mrpt::poses::CPosePDF& o); + + /** Returns a representation of a the PDF - this is just an auxiliary + * function, it's more natural to call + * mrpt::poses::CPointPDF::getAs3DObject */ + static CSetOfObjects::Ptr posePDF2opengl(const mrpt::poses::CPointPDF& o); + + /** Returns a representation of a the PDF - this is just an auxiliary + * function, it's more natural to call + * mrpt::poses::CPose3DPDF::getAs3DObject */ + static CSetOfObjects::Ptr posePDF2opengl(const mrpt::poses::CPose3DPDF& o); + + /** Returns a representation of a the PDF - this is just an auxiliary + * function, it's more natural to call + * mrpt::poses::CPose3DQuatPDF::getAs3DObject */ + static CSetOfObjects::Ptr posePDF2opengl(const mrpt::poses::CPose3DQuatPDF& o); + + /** @} */ +}; +/** Inserts an object into the list. Allows call chaining. + * \sa mrpt::viz::CSetOfObjects::insert + */ +CSetOfObjects::Ptr& operator<<(CSetOfObjects::Ptr& s, const CVisualObject::Ptr& r); + +/** Inserts a set of objects into the list. Allows call chaining. + * \sa mrpt::viz::CSetOfObjects::insert + */ +template +CSetOfObjects::Ptr& operator<<(CSetOfObjects::Ptr& o, const std::vector& v) +{ + o->insertCollection(v); + return o; +} + +// Implementation: (here because it needs the _POST macro defining the +// Smart::Ptr) +template +typename T::Ptr CSetOfObjects::getByClass(size_t ith) const +{ + MRPT_START + size_t foundCount = 0; + for (const auto& o : m_objects) + if (auto obj = std::dynamic_pointer_cast(o); obj) + if (foundCount++ == ith) return obj; + + // If not found directly, search recursively: + for (const auto& o : m_objects) + { + if (auto objs = std::dynamic_pointer_cast(o); objs) + { + typename T::Ptr obj = objs->template getByClass(ith); + if (obj) return obj; + } + } + + return typename T::Ptr(); // Not found: return empty smart pointer + MRPT_END +} +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CSetOfTexturedTriangles.h b/libs/viz/include/mrpt/viz/CSetOfTexturedTriangles.h new file mode 100644 index 0000000000..6e0b396424 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CSetOfTexturedTriangles.h @@ -0,0 +1,76 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +namespace mrpt::viz +{ +/** A set of textured triangles. + * This class can be used to draw any solid, arbitrarily complex object with + * textures. + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CSetOfTexturedTriangles : + virtual public CVisualObject, + public VisualObjectParams_TexturedTriangles +{ + DEFINE_SERIALIZABLE(CSetOfTexturedTriangles, mrpt::viz) + + public: + using TVertex = mrpt::viz::TTriangle::Vertex; + using TTriangle = mrpt::viz::TTriangle; + + CSetOfTexturedTriangles() = default; + virtual ~CSetOfTexturedTriangles() override = default; + + /** Evaluates the bounding box of this object (including possible children) + * in the coordinate frame of the object parent. */ + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + void clearTriangles() + { + { + std::unique_lock writeLock(m_trianglesMtx.data); + m_triangles.clear(); + } + CVisualObject::notifyChange(); + } + size_t getTrianglesCount() const + { + std::shared_lock readLock(m_trianglesMtx.data); + return m_triangles.size(); + } + TTriangle getTriangle(size_t idx) const + { + std::shared_lock readLock(m_trianglesMtx.data); + ASSERT_LT_(idx, m_triangles.size()); + return m_triangles[idx]; + } + void getTriangle(size_t idx, TTriangle& t) const + { + std::shared_lock readLock(m_trianglesMtx.data); + ASSERT_LT_(idx, m_triangles.size()); + t = m_triangles[idx]; + CVisualObject::notifyChange(); + } + void insertTriangle(const TTriangle& t) + { + { + std::unique_lock writeLock(m_trianglesMtx.data); + m_triangles.push_back(t); + } + CVisualObject::notifyChange(); + } + + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CSetOfTriangles.h b/libs/viz/include/mrpt/viz/CSetOfTriangles.h new file mode 100644 index 0000000000..e97db3e744 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CSetOfTriangles.h @@ -0,0 +1,144 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include + +namespace mrpt::viz +{ +/** A set of colored triangles, able to draw any solid, arbitrarily complex + * object without textures. For textures, see CSetOfTexturedTriangles + * + * \sa opengl::Scene, CSetOfTexturedTriangles + * \ingroup mrpt_viz_grp + */ +class CSetOfTriangles : virtual public CVisualObject, public VisualObjectParams_Triangles +{ + DEFINE_SERIALIZABLE(CSetOfTriangles, mrpt::viz) + public: + using const_iterator = std::vector::const_iterator; + using const_reverse_iterator = std::vector::const_reverse_iterator; + + /** Explicitly updates the internal polygon cache, with all triangles as + * polygons. \sa getPolygons() */ + void updatePolygons() const; + + /** Clear this object, removing all triangles. */ + void clearTriangles(); + + /** Get triangle count */ + size_t getTrianglesCount() const; + + /** Gets the i-th triangle */ + void getTriangle(size_t idx, TTriangle& t) const + { + std::shared_lock trisReadLock( + VisualObjectParams_Triangles::m_trianglesMtx.data); + + ASSERT_LT_(idx, shaderTrianglesBuffer().size()); + t = shaderTrianglesBuffer().at(idx); + } + /** Inserts a triangle into the set */ + void insertTriangle(const TTriangle& t) + { + std::unique_lock trisLck(VisualObjectParams_Triangles::m_trianglesMtx.data); + auto& tris = VisualObjectParams_Triangles::m_triangles; + + tris.push_back(t); + polygonsUpToDate = false; + trisLck.unlock(); + CVisualObject::notifyChange(); + } + + /** Inserts a set of triangles, bounded by iterators, into this set. + * \sa insertTriangle + */ + template + void insertTriangles(const InputIterator& begin, const InputIterator& end) + { + std::unique_lock trisLck(VisualObjectParams_Triangles::m_trianglesMtx.data); + auto& tris = VisualObjectParams_Triangles::m_triangles; + + tris.insert(tris.end(), begin, end); + polygonsUpToDate = false; + trisLck.unlock(); + CVisualObject::notifyChange(); + } + + /** Inserts an existing CSetOfTriangles into this one */ + void insertTriangles(const CSetOfTriangles::Ptr& p); + + /** + * Reserves memory for certain number of triangles, avoiding multiple + * memory allocation calls. + */ + void reserve(size_t t) + { + std::unique_lock trisLck(VisualObjectParams_Triangles::m_trianglesMtx.data); + auto& tris = VisualObjectParams_Triangles::m_triangles; + + tris.reserve(t); + trisLck.unlock(); + CVisualObject::notifyChange(); + } + + /** Overwrite all triangles colors with the one provided */ + CVisualObject& setColor_u8(const mrpt::img::TColor& c) override; + /** Overwrite all triangles colors with the one provided */ + CVisualObject& setColorA_u8(const uint8_t a) override; + + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + + /** + * Gets the polygon cache. + * \sa insertTriangles + */ + void getPolygons(std::vector& polys) const; + + /** + * Inserts a set of triangles, given in a container of either TTriangle's + * or TPolygon3D + * \sa insertTriangle + */ + template + void insertTriangles(const CONTAINER& c) + { + this->insertTriangles(c.begin(), c.end()); + CVisualObject::notifyChange(); + } + + /** Evaluates the bounding box of this object (including possible children) + * in the coordinate frame of the object parent. */ + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + CSetOfTriangles() = default; + virtual ~CSetOfTriangles() override = default; + + protected: + /** + * Mutable variable used to check whether polygons need to be recalculated. + */ + mutable bool polygonsUpToDate{false}; + + /** Polygon cache, used for ray-tracing only */ + mutable std::vector m_polygons; +}; +/** Inserts a set of triangles into the list; note that this method allows one + * to pass another CSetOfTriangles as argument. Allows call chaining. \sa + * mrpt::viz::CSetOfTriangles::insertTriangle + */ +template +CSetOfTriangles::Ptr& operator<<(CSetOfTriangles::Ptr& s, const T& t) +{ + s->insertTriangles(t.begin(), t.end()); + return s; +} +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CSimpleLine.h b/libs/viz/include/mrpt/viz/CSimpleLine.h new file mode 100644 index 0000000000..4e47520957 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CSimpleLine.h @@ -0,0 +1,84 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include + +namespace mrpt::viz +{ +/** A line segment + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CSimpleLine : virtual public CVisualObject, public VisualObjectParams_Lines +{ + DEFINE_SERIALIZABLE(CSimpleLine, mrpt::viz) + + protected: + float m_x0, m_y0, m_z0; + float m_x1, m_y1, m_z1; + + public: + void setLineCoords(const mrpt::math::TPoint3Df& p0, const mrpt::math::TPoint3Df& p1) + { + m_x0 = p0.x; + m_y0 = p0.y; + m_z0 = p0.z; + m_x1 = p1.x; + m_y1 = p1.y; + m_z1 = p1.z; + } + + mrpt::math::TPoint3Df getLineStart() const { return {m_x0, m_y0, m_z0}; } + mrpt::math::TPoint3Df getLineEnd() const { return {m_x1, m_y1, m_z1}; } + + /// \deprecated (MRPT 2.3.1) + void setLineCoords(float x0, float y0, float z0, float x1, float y1, float z1) + { + m_x0 = x0; + m_y0 = y0; + m_z0 = z0; + m_x1 = x1; + m_y1 = y1; + m_z1 = z1; + CVisualObject::notifyChange(); + } + + /// \deprecated (MRPT 2.3.1) + void getLineCoords(float& x0, float& y0, float& z0, float& x1, float& y1, float& z1) const + { + x0 = m_x0; + y0 = m_y0; + z0 = m_z0; + x1 = m_x1; + y1 = m_y1; + z1 = m_z1; + } + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Constructor + */ + CSimpleLine( + float x0 = 0, + float y0 = 0, + float z0 = 0, + float x1 = 0, + float y1 = 0, + float z1 = 0, + float lineWidth = 1, + bool antiAliasing = true); + + /** Private, virtual destructor: only can be deleted from smart pointers + */ + ~CSimpleLine() override = default; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CSkyBox.h b/libs/viz/include/mrpt/viz/CSkyBox.h new file mode 100644 index 0000000000..649acffefd --- /dev/null +++ b/libs/viz/include/mrpt/viz/CSkyBox.h @@ -0,0 +1,55 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include +#include + +namespace mrpt::viz +{ +/** A Sky Box: 6 textures that are always rendered at "infinity" to give the + * impression of the scene to be much larger. + * + * Refer to example \ref opengl_skybox_example + * + * + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CSkyBox : public CVisualObject +{ + DEFINE_SERIALIZABLE(CSkyBox, mrpt::viz) + + public: + CSkyBox() = default; + virtual ~CSkyBox() override; + + /** Assigns a texture. It is mandatory to assign all 6 faces before + * initializing/rendering the texture. + * + * \note Images are copied, the original ones can be deleted. + */ + void assignImage(const CUBE_TEXTURE_FACE face, const mrpt::img::CImage& img); + + /// \overload with move semantics for the image. + void assignImage(const CUBE_TEXTURE_FACE face, img::CImage&& img); + + auto internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf override; + + bool cullElegible() const override { return false; } + + private: + /// The cube texture for the 6 faces + std::array m_textureImages; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CSphere.h b/libs/viz/include/mrpt/viz/CSphere.h new file mode 100644 index 0000000000..a883183e21 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CSphere.h @@ -0,0 +1,71 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +namespace mrpt::viz +{ +/** A solid or wire-frame sphere. + * + * ![mrpt::viz::CSphere](preview_CSphere.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CSphere : public CGeneralizedEllipsoidTemplate<3> +{ + using BASE = CGeneralizedEllipsoidTemplate<3>; + DEFINE_SERIALIZABLE(CSphere, mrpt::viz) + + public: + void setRadius(float r) + { + m_radius = r; + CVisualObject::notifyChange(); + } + float getRadius() const { return m_radius; } + void setNumberDivs(int N) + { + m_nDivs = N; + regenerateBaseParams(); + } + + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + virtual mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Constructor */ + CSphere(float radius = 1.0f, int nDivs = 20) : m_radius(radius), m_nDivs(nDivs) + { + regenerateBaseParams(); + BASE::enableDrawSolid3D(true); // default + } + + virtual ~CSphere() override = default; + + protected: + float m_radius; + int m_nDivs; + + void regenerateBaseParams() + { + BASE::setCovMatrix(mrpt::math::CMatrixDouble33::Identity()); + BASE::setQuantiles(m_radius); + BASE::setNumberOfSegments(m_nDivs); + } + + void transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const override + { + out_pts = in_pts; + } +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CText.h b/libs/viz/include/mrpt/viz/CText.h new file mode 100644 index 0000000000..f6f41f229e --- /dev/null +++ b/libs/viz/include/mrpt/viz/CText.h @@ -0,0 +1,70 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include + +namespace mrpt::viz +{ +/** A 2D text that always "faces the observer" despite it having a real 3D + * position, used to compute its position on the screen, and depth (so it can be + * occluded). + * + * Use setString() and setFont() to change the text and its appareance. + * + * ![mrpt::viz::CText](preview_CText.png) + * + * \sa CText3D, opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CText : public CVisualObject +{ + DEFINE_SERIALIZABLE(CText, mrpt::viz) + protected: + std::string m_str; + std::string m_fontName = "sans"; + int m_fontHeight = 20; + + public: + /** Sets the text to display */ + void setString(const std::string& s) + { + if (m_str == s) return; + m_str = s; + CVisualObject::notifyChange(); + } + /** Return the current text associated to this label */ + std::string getString() const { return m_str; } + + /** Sets the font among "sans", "serif", "mono". */ + void setFont(const std::string& s, int height) + { + if (m_fontName == s && m_fontHeight == height) return; + m_fontName = s; + m_fontHeight = height; + CVisualObject::notifyChange(); + } + std::string getFont() const { return m_fontName; } + + /** Evaluates the bounding box of this object (including possible children) + * in the coordinate frame of the object parent. */ + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Constructor */ + CText(const std::string& str = std::string("")) : m_str(str) {} + + virtual ~CText() override; + + std::pair computeTextExtension() const; + + void toYAMLMap(mrpt::containers::yaml& propertiesMap) const override; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CText3D.h b/libs/viz/include/mrpt/viz/CText3D.h new file mode 100644 index 0000000000..fc8270afb3 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CText3D.h @@ -0,0 +1,107 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include +#include + +namespace mrpt::viz +{ +/** A 3D text (rendered with OpenGL primitives), with selectable font face and + * drawing style. + * Use \a setString and \a setFont to change the text displayed by this object + * (can be multi-lined). + * + * Text is drawn along the (+X,+Y) axes. + * + * Default size of characters is "1.0 units". Change it with the standard + * method \a CVisualObject::setScale() as with any other 3D object. + * The color can be also changed with standard methods in the base class \a + * CRenderizable. + * + * ![mrpt::viz::CText3D](preview_CText3D.png) + * + * \sa opengl::Scene, CText + * \note This class is based on code from libcvd (BSD, + * http://www.edwardrosten.com/cvd/ ) \ingroup mrpt_viz_grp + */ +class CText3D : public CVisualObject +{ + DEFINE_SERIALIZABLE(CText3D, mrpt::viz) + protected: + std::string m_str; + std::string m_fontName = "sans"; + TOpenGLFontStyle m_text_style; + double m_text_spacing = 1.5; + double m_text_kerning = 0.1; + + public: + CText3D( + const std::string& str = std::string(""), + const std::string& fontName = std::string("sans"), + const float scale = 1.0, + const mrpt::viz::TOpenGLFontStyle text_style = mrpt::viz::NICE, + const double text_spacing = 1.5, + const double text_kerning = 0.1); + + ~CText3D() override; + + /** Sets the displayed string */ + void setString(const std::string& s) + { + m_str = s; + CVisualObject::notifyChange(); + } + + /** Returns the currently text associated to this object */ + [[nodiscard]] const std::string& getString() const { return m_str; } + + /** Changes the font name, among accepted values: "sans", "mono", "serif" */ + void setFont(const std::string& font) + { + m_fontName = font; + CVisualObject::notifyChange(); + } + + /** Returns the text font */ + [[nodiscard]] const std::string& getFont() const { return m_fontName; } + + /** Change drawing style: FILL, OUTLINE, NICE */ + void setTextStyle(const mrpt::viz::TOpenGLFontStyle text_style) + { + m_text_style = text_style; + CVisualObject::notifyChange(); + } + + /** Gets the current drawing style */ + [[nodiscard]] mrpt::viz::TOpenGLFontStyle getTextStyle() const { return m_text_style; } + + void setTextSpacing(const double text_spacing) + { + m_text_spacing = text_spacing; + CVisualObject::notifyChange(); + } + + [[nodiscard]] double setTextSpacing() const { return m_text_spacing; } + + void setTextKerning(const double text_kerning) + { + m_text_kerning = text_kerning; + CVisualObject::notifyChange(); + } + + [[nodiscard]] double setTextKerning() const { return m_text_kerning; } + + [[nodiscard]] mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + void toYAMLMap(mrpt::containers::yaml& propertiesMap) const override; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CTextMessageCapable.h b/libs/viz/include/mrpt/viz/CTextMessageCapable.h new file mode 100644 index 0000000000..13a28e0414 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CTextMessageCapable.h @@ -0,0 +1,86 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include + +#include +#include + +namespace mrpt::viz +{ +/** Keeps a list of text messages which can be rendered to OpenGL contexts by + * graphic classes. + * \ingroup mrpt_viz_grp + */ +class CTextMessageCapable +{ + public: + void clearTextMessages(); + + /** Add 2D text messages overlapped to the 3D rendered scene. The string + * will remain displayed in the 3D window + * until it's changed with subsequent calls to this same method, or all + * the texts are cleared with clearTextMessages(). + * + * \param x The X position, interpreted as absolute pixels from the left + * if X>=1, absolute pixels from the left if X<0 or as a width factor if in + * the range [0,1[. + * \param y The Y position, interpreted as absolute pixels from the bottom + * if Y>=1, absolute pixels from the top if Y<0 or as a height factor if in + * the range [0,1[. + * \param text The text string to display. + * \param color The text color. For example: TColorf(1.0,1.0,1.0) + * \param unique_index An "index" for this text message, so that + * subsequent calls with the same index will overwrite this text message + * instead of creating new ones. + * + * You'll need to refresh the display manually with forceRepaint(). + * + * \sa clearTextMessages, updateTextMessage + */ + void addTextMessage( + const double x_frac, + const double y_frac, + const std::string& text, + const size_t unique_index = 0, + const TFontParams& fontParams = TFontParams()); + + /** Just updates the text of a given text message, without touching the + * other parameters. + * \return false if given ID doesn't exist. + */ + bool updateTextMessage(size_t unique_index, const std::string& text); + + struct DataPerText : mrpt::viz::T2DTextData + { + mutable mrpt::viz::CText::Ptr gl_text, gl_text_shadow; + mutable bool gl_text_outdated = true; + }; + + struct TListTextMessages + { + mutable mrpt::containers::NonCopiableData mtx; + + std::map messages; + + /** (re)generate all CText objects in the gl_text fields */ + void regenerateGLobjects() const; + }; + + const TListTextMessages& getTextMessages() const { return m_2D_texts; } + + protected: + TListTextMessages m_2D_texts; + +}; // end of CTextMessageCapable + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CTexturedPlane.h b/libs/viz/include/mrpt/viz/CTexturedPlane.h new file mode 100644 index 0000000000..1d3e9f075c --- /dev/null +++ b/libs/viz/include/mrpt/viz/CTexturedPlane.h @@ -0,0 +1,79 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include + +namespace mrpt::viz +{ +/** A 2D plane in the XY plane with a texture image. + * + * Lighting is disabled by default in this class, so the plane color or texture + * will be independent of its orientation or shadows cast on it. + * This can be changed calling enableLighting(true) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ +class CTexturedPlane : + virtual public CVisualObject, + public VisualObjectParams_Triangles, + public VisualObjectParams_TexturedTriangles +{ + DEFINE_SERIALIZABLE(CTexturedPlane, mrpt::viz) + + protected: + float m_xMin = -1.0f, m_xMax = 1.0f; + float m_yMin = -1.0f, m_yMax = 1.0f; + + mutable bool polygonUpToDate{false}; + /** Used for ray-tracing */ + mutable std::vector tmpPoly; + void updatePoly() const; + + public: + CTexturedPlane(float x_min = -1, float x_max = 1, float y_min = -1, float y_max = 1); + virtual ~CTexturedPlane() override = default; + + /** Set the coordinates of the four corners that define the plane on the XY + * plane. */ + void setPlaneCorners(float xMin, float xMax, float yMin, float yMax) + { + ASSERT_NOT_EQUAL_(xMin, xMax); + ASSERT_NOT_EQUAL_(yMin, yMax); + m_xMin = xMin; + m_xMax = xMax; + m_yMin = yMin; + m_yMax = yMax; + polygonUpToDate = false; + CVisualObject::notifyChange(); + } + + /** Get the coordinates of the four corners that define the plane on the XY + * plane. */ + void getPlaneCorners(float& xMin, float& xMax, float& yMin, float& yMax) const + { + xMin = m_xMin; + xMax = m_xMax; + yMin = m_yMin; + yMax = m_yMax; + } + + void enableLighting(bool enable = true) + { + VisualObjectParams_TexturedTriangles::enableLight(enable); + VisualObjectParams_Triangles::enableLight(enable); + } + + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override; + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CUBE_TEXTURE_FACE.h b/libs/viz/include/mrpt/viz/CUBE_TEXTURE_FACE.h new file mode 100644 index 0000000000..af0441ad88 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CUBE_TEXTURE_FACE.h @@ -0,0 +1,37 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +namespace mrpt::viz +{ +/** Enum type for each of the 6 faces of a Cube Texture. + * + * Note that these enums must be defined in the same order than OpenGL API + * constants: + * + * #define GL_TEXTURE_CUBE_MAP_POSITIVE_X 0x8515 + * #define GL_TEXTURE_CUBE_MAP_NEGATIVE_X 0x8516 + * #define GL_TEXTURE_CUBE_MAP_POSITIVE_Y 0x8517 + * #define GL_TEXTURE_CUBE_MAP_NEGATIVE_Y 0x8518 + * #define GL_TEXTURE_CUBE_MAP_POSITIVE_Z 0x8519 + * #define GL_TEXTURE_CUBE_MAP_NEGATIVE_Z 0x851A + * + * \ingroup mrpt_viz_grp + */ +enum class CUBE_TEXTURE_FACE +{ + LEFT = 0, // +X + RIGHT, // -X + TOP, // +Y + BOTTOM, // -Y + FRONT, // +Z + BACK // -Z +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CVectorField2D.h b/libs/viz/include/mrpt/viz/CVectorField2D.h new file mode 100644 index 0000000000..fddde891d4 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CVectorField2D.h @@ -0,0 +1,195 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include +#include +#include + +namespace mrpt::viz +{ +/** A 2D vector field representation, consisting of points and arrows drawn on a + * plane (invisible grid). + * + * ![mrpt::viz::CVectorField2D](preview_CVectorField2D.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ + +class CVectorField2D : + virtual public CVisualObject, + public VisualObjectParams_Lines, + public VisualObjectParams_Points, + public VisualObjectParams_Triangles +{ + DEFINE_SERIALIZABLE(CVectorField2D, mrpt::viz) + protected: + /** X component of the vector field */ + mrpt::math::CMatrixF xcomp; + /** Y component of the vector field */ + mrpt::math::CMatrixF ycomp; + + /** Grid bounds */ + float xMin{-1.0f}, xMax{1.0f}, yMin{-1.0f}, yMax{1.0f}; + + mrpt::img::TColor m_point_color; + mrpt::img::TColor m_field_color; + + public: + /** + * Clear the matrices + */ + void clear() + { + xcomp.resize(0, 0); + ycomp.resize(0, 0); + CVisualObject::notifyChange(); + } + + /** + * Set the point color in the range [0,1] + */ + void setPointColor(const float R, const float G, const float B, const float A = 1) + { + m_point_color = mrpt::img::TColor(f2u8(R), f2u8(G), f2u8(B), f2u8(A)); + CVisualObject::notifyChange(); + } + + /** + * Get the point color in the range [0,1] + */ + mrpt::img::TColorf getPointColor() const { return mrpt::img::TColorf(m_point_color); } + + /** + * Set the arrow color in the range [0,1] + */ + void setVectorFieldColor(const float R, const float G, const float B, const float A = 1) + { + m_field_color = mrpt::img::TColor(f2u8(R), f2u8(G), f2u8(B), f2u8(A)); + CVisualObject::notifyChange(); + } + + /** + * Get the arrow color in the range [0,1] + */ + mrpt::img::TColorf getVectorFieldColor() const { return mrpt::img::TColorf(m_field_color); } + + /** + * Set the coordinates of the grid on where the vector field will be drawn + * by setting its center and the cell size. + * The number of cells is marked by the content of xcomp and ycomp. + * \sa xcomp, ycomp + */ + void setGridCenterAndCellSize( + const float center_x, const float center_y, const float cellsize_x, const float cellsize_y) + { + xMin = center_x - 0.5f * cellsize_x * (xcomp.cols() - 1); + xMax = center_x + 0.5f * cellsize_x * (xcomp.cols() - 1); + yMin = center_y - 0.5f * cellsize_y * (xcomp.rows() - 1); + yMax = center_y + 0.5f * cellsize_y * (xcomp.rows() - 1); + CVisualObject::notifyChange(); + } + + /** + * Set the coordinates of the grid on where the vector field will be drawn + * using x-y max and min values. + */ + void setGridLimits(const float xmin, const float xmax, const float ymin, const float ymax) + { + xMin = xmin; + xMax = xmax; + yMin = ymin; + yMax = ymax; + CVisualObject::notifyChange(); + } + + /** + * Get the coordinates of the grid on where the vector field is drawn using + * the max and min values. + */ + void getGridLimits(float& xmin, float& xmax, float& ymin, float& ymax) const + { + xmin = xMin; + xmax = xMax; + ymin = yMin; + ymax = yMax; + } + + /** + * Get the vector field. Matrix_x stores the "x" component and Matrix_y + * stores the "y" component. + */ + void getVectorField(mrpt::math::CMatrixFloat& Matrix_x, mrpt::math::CMatrixFloat& Matrix_y) const + { + Matrix_x = xcomp; + Matrix_y = ycomp; + } + + /** Get the "x" component of the vector field, as a matrix where each entry + * represents a point in the 2D grid. */ + const mrpt::math::CMatrixFloat& getVectorField_x() const { return xcomp; } + /** \overload */ + mrpt::math::CMatrixFloat& getVectorField_x() { return xcomp; } + /** Get the "y" component of the vector field, as a matrix where each entry + * represents a point in the 2D grid. */ + const mrpt::math::CMatrixFloat& getVectorField_y() const { return ycomp; } + /** \overload */ + mrpt::math::CMatrixFloat& getVectorField_y() { return ycomp; } + /** + * Set the vector field. Matrix_x contains the "x" component and Matrix_y + * contains the "y" component. + */ + void setVectorField(mrpt::math::CMatrixFloat& Matrix_x, mrpt::math::CMatrixFloat& Matrix_y) + { + ASSERT_((Matrix_x.rows() == Matrix_y.rows()) && (Matrix_x.cols() == Matrix_y.cols())); + xcomp = Matrix_x; + ycomp = Matrix_y; + CVisualObject::notifyChange(); + } + + /** + * Adjust the vector field in the scene (vectors magnitude) according to + * the grid size. + */ + void adjustVectorFieldToGrid(); + + /** Resizes the set. + */ + void resize(size_t rows, size_t cols) + { + xcomp.resize(rows, cols); + ycomp.resize(rows, cols); + CVisualObject::notifyChange(); + } + + /** Returns the total count of rows used to represent the vector field. */ + size_t cols() const { return xcomp.cols(); } + /** Returns the total count of columns used to represent the vector field. + */ + size_t rows() const { return xcomp.rows(); } + + mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + /** Constructor */ + CVectorField2D(); + /** Constructor with a initial set of lines. */ + CVectorField2D( + mrpt::math::CMatrixFloat Matrix_x, + mrpt::math::CMatrixFloat Matrix_y, + float xmin = -1, + float xmax = 1, + float ymin = -1, + float ymax = 1); + /** Private, virtual destructor: only can be deleted from smart pointers. */ + ~CVectorField2D() override = default; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CVectorField3D.h b/libs/viz/include/mrpt/viz/CVectorField3D.h new file mode 100644 index 0000000000..dd29f8b192 --- /dev/null +++ b/libs/viz/include/mrpt/viz/CVectorField3D.h @@ -0,0 +1,312 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include +#include + +namespace mrpt::viz +{ +/** A 3D vector field representation, consisting of points and arrows drawn at + * any spatial position. + * This opengl object has been created to represent scene flow, and hence + * both the vector field and + * the coordinates of the points at which the vector field is represented + * are stored in matrices because + * they are computed from intensity and depth images. + * + * ![mrpt::viz::CVectorField3D](preview_CVectorField3D.png) + * + * \sa opengl::Scene + * \ingroup mrpt_viz_grp + */ + +class CVectorField3D : + virtual public CVisualObject, + public VisualObjectParams_Points, + public VisualObjectParams_Lines +{ + DEFINE_SERIALIZABLE(CVectorField3D, mrpt::viz) + public: + CVectorField3D(); + CVectorField3D( + mrpt::math::CMatrixFloat x_vf_ini, + mrpt::math::CMatrixFloat y_vf_ini, + mrpt::math::CMatrixFloat z_vf_ini, + mrpt::math::CMatrixFloat x_p_ini, + mrpt::math::CMatrixFloat y_p_ini, + mrpt::math::CMatrixFloat z_p_ini); + + ~CVectorField3D() override = default; + + /** + * Clear the matrices + */ + void clear() + { + x_vf.resize(0, 0); + y_vf.resize(0, 0); + z_vf.resize(0, 0); + x_p.resize(0, 0); + y_p.resize(0, 0); + z_p.resize(0, 0); + + CVisualObject::notifyChange(); + } + + /** + * Set the point color in the range [0,1] + */ + void setPointColor(const float R, const float G, const float B, const float A = 1) + { + m_point_color = mrpt::img::TColor(f2u8(R), f2u8(G), f2u8(B), f2u8(A)); + CVisualObject::notifyChange(); + } + + /** + * Get the point color in the range [0,1] + */ + mrpt::img::TColorf getPointColor() const { return mrpt::img::TColorf(m_point_color); } + + /** + * Set the arrow color in the range [0,1] + */ + void setVectorFieldColor(const float R, const float G, const float B, const float A = 1) + { + m_field_color = mrpt::img::TColor(f2u8(R), f2u8(G), f2u8(B), f2u8(A)); + CVisualObject::notifyChange(); + } + + /** + * Get the motion field min and max colors (colormap) in the range [0,1] + */ + void getVectorFieldColor(mrpt::img::TColorf Cmin, mrpt::img::TColorf Cmax) const + { + Cmin = mrpt::img::TColorf{m_still_color}; + Cmax = mrpt::img::TColorf{m_maxspeed_color}; + } + + /** + * Set the motion field min and max colors (colormap) in the range [0,1] + */ + void setMotionFieldColormap( + const float Rmin, + const float Gmin, + const float Bmin, + const float Rmax, + const float Gmax, + const float Bmax, + const float Amin = 1, + const float Amax = 1) + { + m_still_color = mrpt::img::TColor(f2u8(Rmin), f2u8(Gmin), f2u8(Bmin), f2u8(Amin)); + m_maxspeed_color = mrpt::img::TColor(f2u8(Rmax), f2u8(Gmax), f2u8(Bmax), f2u8(Amax)); + CVisualObject::notifyChange(); + } + + /** + * Get the arrow color in the range [0,1] + */ + mrpt::img::TColorf getVectorFieldColor() const { return mrpt::img::TColorf(m_field_color); } + + /** + * Set the max speed associated for the color map ( m_still_color, + * m_maxspeed_color) + */ + void setMaxSpeedForColor(const float s) + { + m_maxspeed = s; + CVisualObject::notifyChange(); + } + + /** + * Get the max_speed with which lines are drawn. + */ + float getMaxSpeedForColor() const { return m_maxspeed; } + /** + * Get the vector field in three independent matrices: Matrix_x, Matrix_y + * and Matrix_z. + */ + void getVectorField( + mrpt::math::CMatrixFloat& Matrix_x, + mrpt::math::CMatrixFloat& Matrix_y, + mrpt::math::CMatrixFloat& Matrix_z) const + { + Matrix_x = x_vf; + Matrix_y = y_vf; + Matrix_z = z_vf; + } + + template + void getVectorField(MATRIX& Matrix_x, MATRIX& Matrix_y, MATRIX& Matrix_z) const + { + Matrix_x = x_vf; + Matrix_y = y_vf; + Matrix_z = z_vf; + } + + /** + * Get the coordiantes of the points at which the vector field is + * plotted: Coord_x, Coord_y and Coord_z. + */ + void getPointCoordinates( + mrpt::math::CMatrixFloat& Coord_x, + mrpt::math::CMatrixFloat& Coord_y, + mrpt::math::CMatrixFloat& Coord_z) const + { + Coord_x = x_p; + Coord_y = y_p; + Coord_z = z_p; + } + + template + void getPointCoordinates(MATRIX& Coord_x, MATRIX& Coord_y, MATRIX& Coord_z) const + { + Coord_x = x_p; + Coord_y = y_p; + Coord_z = z_p; + } + + /** Get the "x" component of the vector field as a matrix. */ + const mrpt::math::CMatrixFloat& getVectorField_x() const { return x_vf; } + /** \overload */ + mrpt::math::CMatrixFloat& getVectorField_x() { return x_vf; } + /** Get the "y" component of the vector field as a matrix. */ + const mrpt::math::CMatrixFloat& getVectorField_y() const { return y_vf; } + /** \overload */ + mrpt::math::CMatrixFloat& getVectorField_y() { return y_vf; } + /** Get the "z" component of the vector field as a matrix. */ + const mrpt::math::CMatrixFloat& getVectorField_z() const { return z_vf; } + /** \overload */ + mrpt::math::CMatrixFloat& getVectorField_z() { return z_vf; } + /** + * Set the vector field with Matrix_x, Matrix_y and Matrix_z. + */ + void setVectorField( + mrpt::math::CMatrixFloat& Matrix_x, + mrpt::math::CMatrixFloat& Matrix_y, + mrpt::math::CMatrixFloat& Matrix_z) + { + ASSERT_((Matrix_x.rows() == Matrix_y.rows()) && (Matrix_x.rows() == Matrix_z.rows())); + ASSERT_((Matrix_x.cols() == Matrix_y.cols()) && (Matrix_x.cols() == Matrix_z.cols())); + x_vf = Matrix_x; + y_vf = Matrix_y; + z_vf = Matrix_z; + CVisualObject::notifyChange(); + } + + template + void setVectorField(MATRIX& Matrix_x, MATRIX& Matrix_y, MATRIX& Matrix_z) + { + ASSERT_((Matrix_x.rows() == Matrix_y.rows()) && (Matrix_x.rows() == Matrix_z.rows())); + ASSERT_((Matrix_x.cols() == Matrix_y.cols()) && (Matrix_x.cols() == Matrix_z.cols())); + x_vf = Matrix_x; + y_vf = Matrix_y; + z_vf = Matrix_z; + CVisualObject::notifyChange(); + } + + /** + * Set the coordinates of the points at which the vector field is plotted + * with Matrix_x, Matrix_y and Matrix_z. + */ + void setPointCoordinates( + mrpt::math::CMatrixFloat& Matrix_x, + mrpt::math::CMatrixFloat& Matrix_y, + mrpt::math::CMatrixFloat& Matrix_z) + { + ASSERT_((Matrix_x.rows() == Matrix_y.rows()) && (Matrix_x.rows() == Matrix_z.rows())); + ASSERT_((Matrix_x.cols() == Matrix_y.cols()) && (Matrix_x.cols() == Matrix_z.cols())); + x_p = Matrix_x; + y_p = Matrix_y; + z_p = Matrix_z; + CVisualObject::notifyChange(); + } + + template + void setPointCoordinates(MATRIX& Matrix_x, MATRIX& Matrix_y, MATRIX& Matrix_z) + { + ASSERT_((Matrix_x.rows() == Matrix_y.rows()) && (Matrix_x.rows() == Matrix_z.rows())); + ASSERT_((Matrix_x.cols() == Matrix_y.cols()) && (Matrix_x.cols() == Matrix_z.cols())); + x_p = Matrix_x; + y_p = Matrix_y; + z_p = Matrix_z; + CVisualObject::notifyChange(); + } + + /** + * Resizes the set. + */ + void resize(size_t rows, size_t cols) + { + x_vf.resize(rows, cols); + y_vf.resize(rows, cols); + z_vf.resize(rows, cols); + x_p.resize(rows, cols); + y_p.resize(rows, cols); + z_p.resize(rows, cols); + CVisualObject::notifyChange(); + } + + /** Returns the total count of rows used to represent the vector field. */ + [[nodiscard]] size_t cols() const { return x_vf.cols(); } + + /** Returns the total count of columns used to represent the vector field. */ + [[nodiscard]] size_t rows() const { return x_vf.rows(); } + + [[nodiscard]] mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const override; + + void enableColorFromModule(bool enable = true) + { + m_colorFromModule = enable; + CVisualObject::notifyChange(); + } + void enableShowPoints(bool enable = true) + { + m_showPoints = enable; + CVisualObject::notifyChange(); + } + + [[nodiscard]] bool isColorFromModuleEnabled() const { return m_colorFromModule; } + + protected: + /** X component of the vector field */ + mrpt::math::CMatrixF x_vf; + /** Y component of the vector field */ + mrpt::math::CMatrixF y_vf; + /** Z component of the vector field */ + mrpt::math::CMatrixF z_vf; + + /** X coordinate of the points at which the vector field is plotted */ + mrpt::math::CMatrixF x_p; + /** Y coordinate of the points at which the vector field is plotted */ + mrpt::math::CMatrixF y_p; + /** Z coordinate of the points at which the vector field is plotted */ + mrpt::math::CMatrixF z_p; + + /** By default it is false */ + bool m_colorFromModule{false}; + /** By default it is true */ + bool m_showPoints{true}; + + mrpt::img::TColor m_point_color; + mrpt::img::TColor m_field_color; + + /** Color associated to fields with null module */ + mrpt::img::TColor m_still_color; + /** Color associated to fields whose module is equal or larger than + * 'm_maxspeed' */ + mrpt::img::TColor m_maxspeed_color; + /** Value of the module of the motion field which will correspond to + * 'm_maxspeed_color' */ + float m_maxspeed; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/CVisualObject.h b/libs/viz/include/mrpt/viz/CVisualObject.h new file mode 100644 index 0000000000..f03f6bf52d --- /dev/null +++ b/libs/viz/include/mrpt/viz/CVisualObject.h @@ -0,0 +1,666 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace mrpt::viz +{ +/** Enum for cull face modes in triangle-based shaders. + * \sa CVisualObjectShaderTriangles, CVisualObjectShaderTexturedTriangles + * \ingroup mrpt_viz_grp + */ +enum class TCullFace : uint8_t +{ + /** The default: culls none, so all front and back faces are visible. */ + NONE = 0, + /** Skip back faces (those that are NOT seen in the CCW direction) */ + BACK, + /** Skip front faces (those that ARE seen in the CCW direction) */ + FRONT +}; + +/** The base class of 3D objects that can be directly rendered through OpenGL. + * In this class there are a set of common properties to all 3D objects, + *mainly: + * - Its SE(3) pose (x,y,z,yaw,pitch,roll), relative to the parent object, + * or the global frame of reference for root objects (inserted into a + *mrpt::viz::Scene). + * - A name: A name that can be optionally asigned to objects for + *easing its reference. + * - A RGBA color: This field will be used in simple elements (points, + *lines, text,...) but is ignored in more complex objects that carry their own + *color information (triangle sets,...) + * - Shininess: See materialShininess(float) + * + * See the main class opengl::Scene + * + * \sa opengl::Scene, mrpt::viz + * \ingroup mrpt_viz_grp + */ +class CVisualObject : public mrpt::serialization::CSerializable +{ + DEFINE_VIRTUAL_SERIALIZABLE(CVisualObject, mrpt::viz) + + friend class mrpt::viz::Viewport; + friend class mrpt::viz::CSetOfObjects; + + protected: + struct State + { + std::string name = {}; + bool show_name = false; + + /** RGBA components in the range [0,255] */ + mrpt::img::TColor color = {0xff, 0xff, 0xff, 0xff}; + + float materialShininess = 0.2f; + + /** SE(3) pose wrt the parent coordinate reference. This class + * automatically holds the cached 3x3 rotation matrix for quick load + * into opengl stack. */ + mrpt::poses::CPose3D pose; + + /** Scale components to apply to the object (default=1) */ + float scale_x = 1.0f, scale_y = 1.0f, scale_z = 1.0f; + + bool visible = true; //!< Is the object visible? (default=true) + + bool castShadows = true; + + mrpt::math::TPoint3Df representativePoint{0, 0, 0}; + }; + + /// All relevant rendering state that needs to get protected by m_stateMtx + State m_state; + mutable mrpt::containers::NonCopiableData m_stateMtx; + + public: + /** @name Changes the appearance of the object to render + @{ */ + + /** Changes the name of the object */ + void setName(const std::string& n) + { + std::unique_lock lckWrite(m_stateMtx.data); + m_state.name = n; + } + /** Returns the name of the object */ + std::string getName() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.name; + } + + /** Is the object visible? \sa setVisibility */ + bool isVisible() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.visible; + } + /** Set object visibility (default=true) \sa isVisible */ + void setVisibility(bool visible = true) + { + std::unique_lock lckWrite(m_stateMtx.data); + m_state.visible = visible; + } + + /** Does the object cast shadows? (default=true) */ + bool castShadows() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.castShadows; + } + /** Enable/disable casting shadows by this object (default=true) */ + void castShadows(bool doCast = true) + { + std::unique_lock lckWrite(m_stateMtx.data); + m_state.castShadows = doCast; + } + + /** Enables or disables showing the name of the object as a label when + * rendering */ + void enableShowName(bool showName = true) + { + std::unique_lock lckWrite(m_stateMtx.data); + m_state.show_name = showName; + } + /** \sa enableShowName */ + bool isShowNameEnabled() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.show_name; + } + + /** Defines the SE(3) (pose=translation+rotation) of the object with respect + * to its parent */ + CVisualObject& setPose(const mrpt::poses::CPose3D& o); + /// \overload + CVisualObject& setPose(const mrpt::poses::CPose2D& o); + /// \overload + CVisualObject& setPose(const mrpt::math::TPose3D& o); + /// \overload + CVisualObject& setPose(const mrpt::math::TPose2D& o); + /// \overload + CVisualObject& setPose(const mrpt::poses::CPoint3D& o); + /// \overload + CVisualObject& setPose(const mrpt::poses::CPoint2D& o); + + /** Returns the 3D pose of the object as TPose3D */ + mrpt::math::TPose3D getPose() const; + + /** Returns a const ref to the 3D pose of the object as mrpt::poses::CPose3D + * (which explicitly contains the 3x3 rotation matrix) */ + mrpt::poses::CPose3D getCPose() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.pose; + } + + /** Changes the location of the object, keeping untouched the orientation + * \return a ref to this */ + CVisualObject& setLocation(double x, double y, double z) + { + std::unique_lock lckWrite(m_stateMtx.data); + m_state.pose.x(x); + m_state.pose.y(y); + m_state.pose.z(z); + return *this; + } + + /** Changes the location of the object, keeping untouched the orientation + * \return a ref to this */ + CVisualObject& setLocation(const mrpt::math::TPoint3D& p) + { + std::unique_lock lckWrite(m_stateMtx.data); + m_state.pose.x(p.x); + m_state.pose.y(p.y); + m_state.pose.z(p.z); + return *this; + } + + /** Get color components as floats in the range [0,1] */ + mrpt::img::TColorf getColor() const + { + std::shared_lock lckRead(m_stateMtx.data); + return mrpt::img::TColorf(m_state.color); + } + + /** Get color components as uint8_t in the range [0,255] */ + mrpt::img::TColor getColor_u8() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.color; + } + + /** Set alpha (transparency) color component in the range [0,1] + * \return a ref to this */ + CVisualObject& setColorA(const float a) { return setColorA_u8(f2u8(a)); } + + /** Set alpha (transparency) color component in the range [0,255] + * \return a ref to this */ + virtual CVisualObject& setColorA_u8(const uint8_t a) + { + m_stateMtx.data.lock(); + m_state.color.A = a; + m_stateMtx.data.unlock(); + notifyChange(); + return *this; + } + + /** Material shininess (for specular lights in shaders that support it), + * between 0.0f (none) to 1.0f (shiny) */ + float materialShininess() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.materialShininess; + } + + /** Material shininess (for specular lights in shaders that support it), + * between 0.0f (none) to 1.0f (shiny) */ + void materialShininess(float shininess) + { + std::unique_lock lckWrite(m_stateMtx.data); + m_state.materialShininess = shininess; + } + + /** Scale to apply to the object, in all three axes (default=1) \return a + * ref to this */ + CVisualObject& setScale(float s) + { + m_stateMtx.data.lock(); + m_state.scale_x = m_state.scale_y = m_state.scale_z = s; + m_stateMtx.data.unlock(); + notifyChange(); + return *this; + } + + /** Scale to apply to the object in each axis (default=1) \return a ref to + * this */ + CVisualObject& setScale(float sx, float sy, float sz) + { + m_stateMtx.data.lock(); + m_state.scale_x = sx; + m_state.scale_y = sy; + m_state.scale_z = sz; + m_stateMtx.data.unlock(); + notifyChange(); + return *this; + } + /** Get the current scaling factor in one axis */ + float getScaleX() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.scale_x; + } + /** Get the current scaling factor in one axis */ + float getScaleY() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.scale_y; + } + /** Get the current scaling factor in one axis */ + float getScaleZ() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.scale_z; + } + + /** Changes the default object color \return a ref to this */ + CVisualObject& setColor(const mrpt::img::TColorf& c) + { + return setColor_u8(mrpt::img::TColor(f2u8(c.R), f2u8(c.G), f2u8(c.B), f2u8(c.A))); + } + + /** Set the color components of this object (R,G,B,Alpha, in the range 0-1) + * \return a ref to this */ + CVisualObject& setColor(float R, float G, float B, float A = 1) + { + return setColor_u8(f2u8(R), f2u8(G), f2u8(B), f2u8(A)); + } + + /*** Changes the default object color \return a ref to this */ + virtual CVisualObject& setColor_u8(const mrpt::img::TColor& c); + + /** Set the color components of this object (R,G,B,Alpha, in the range + * 0-255) \return a ref to this */ + CVisualObject& setColor_u8(uint8_t R, uint8_t G, uint8_t B, uint8_t A = 255) + { + return setColor_u8(mrpt::img::TColor(R, G, B, A)); + } + + /** @} */ + + /** Return false if this object should never be checked for being culled out + * (=not rendered if its bbox are out of the screen limits). + * For example, skyboxes or other special effects. + */ + virtual bool cullElegible() const { return true; } + + /** Used from Scene::asYAML(). + * \note (New in MRPT 2.4.2) */ + virtual void toYAMLMap(mrpt::containers::yaml& propertiesMap) const; + + /** Default constructor: */ + CVisualObject() = default; + ~CVisualObject() override; + + /** Should return true if enqueueForRenderRecursive() is defined since + * the object has inner children. Examples: CSetOfObjects, CAssimpModel. + */ + virtual bool isCompositeObject() const { return false; } + + /** Call to enable calling renderUpdateBuffers() before the next + * render() rendering iteration. */ + void notifyChange() const + { + std::unique_lock lckWrite(m_outdatedStateMtx.data); + m_cachedLocalBBox.reset(); + const_cast(*this).m_outdatedBuffersState.run_on_all( + [](auto& state) { state.outdatedBuffers = true; }); + } + + void notifyBBoxChange() const { m_cachedLocalBBox.reset(); } + + /** Returns whether notifyChange() has been invoked since the last call + * to renderUpdateBuffers(), meaning the latter needs to be called again + * before rendering. + */ + bool hasToUpdateBuffers() const + { + std::shared_lock lckWrite(m_outdatedStateMtx.data); + return m_outdatedBuffersState.get().outdatedBuffers; + } + + /** Simulation of ray-trace, given a pose. Returns true if the ray + * effectively collisions with the object (returning the distance to the + * origin of the ray in "dist"), or false in other case. "dist" variable + * yields undefined behaviour when false is returned + */ + virtual bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const; + + /** Evaluates the bounding box of this object (including possible + * children) in the coordinate frame of my parent object, + * i.e. if this object pose changes, the bbox returned here will change too. + * This is in contrast with the local bbox returned by getBoundingBoxLocal() + */ + auto getBoundingBox() const -> mrpt::math::TBoundingBox + { + return getBoundingBoxLocal().compose(getCPose()); + } + + /** Evaluates the bounding box of this object (including possible + * children) in the coordinate frame of my parent object, + * i.e. if this object pose changes, the bbox returned here will change too. + * This is in contrast with the local bbox returned by getBoundingBoxLocal() + */ + auto getBoundingBoxLocal() const -> mrpt::math::TBoundingBox; + + /// \overload Fastest method, returning a copy of the float version of + /// the bbox. const refs are not returned for multi-thread safety. + auto getBoundingBoxLocalf() const -> mrpt::math::TBoundingBoxf; + + /** Provide a representative point (in object local coordinates), used to + * sort objects by eye-distance while rendering with transparencies + * (Default=[0,0,0]) */ + virtual mrpt::math::TPoint3Df getLocalRepresentativePoint() const + { + std::shared_lock lckRead(m_stateMtx.data); + return m_state.representativePoint; + } + + /** See getLocalRepresentativePoint() */ + void setLocalRepresentativePoint(const mrpt::math::TPoint3Df& p) + { + std::unique_lock lckWrite(m_stateMtx.data); + m_state.representativePoint = p; + } + + /** Returns or constructs (in its first invocation) the associated + * mrpt::viz::CText object representing the label of the object. + * \sa enableShowName() + */ + mrpt::viz::CText& labelObject() const; + + protected: + void writeToStreamRender(mrpt::serialization::CArchive& out) const; + void readFromStreamRender(mrpt::serialization::CArchive& in); + + /** Must be implemented by derived classes to provide the updated bounding + * box in the object local frame of coordinates. + * This will be called only once after each time the derived class reports + * to notifyChange() that the object geometry changed. + * + * \sa getBoundingBox(), getBoundingBoxLocal(), getBoundingBoxLocalf() + */ + [[nodiscard]] virtual mrpt::math::TBoundingBoxf internalBoundingBoxLocal() const = 0; + + struct OutdatedState + { + bool outdatedBuffers = true; + }; + mrpt::containers::PerThreadDataHolder m_outdatedBuffersState; + mutable mrpt::containers::NonCopiableData m_outdatedStateMtx; + + mutable std::optional m_cachedLocalBBox; + + /** Optional pointer to a mrpt::viz::CText */ + mutable std::shared_ptr m_label_obj; +}; + +/** A list of smart pointers to renderizable objects */ +using ListVisualObjects = std::deque; + +class VisualObjectParams_Triangles : public virtual CVisualObject +{ + public: + VisualObjectParams_Triangles() = default; + virtual ~VisualObjectParams_Triangles() = default; + + [[nodiscard]] bool isLightEnabled() const { return m_enableLight; } + void enableLight(bool enable = true) + { + m_enableLight = enable; + CVisualObject::notifyChange(); + } + + /** Control whether to render the FRONT, BACK, or BOTH (default) set of + * faces. Refer to docs for glCullFace(). + * Example: If set to `cullFaces(TCullFace::BACK);`, back faces will not be + * drawn ("culled") + */ + void cullFaces(const TCullFace& cf) + { + m_cullface = cf; + CVisualObject::notifyChange(); + } + [[nodiscard]] TCullFace cullFaces() const { return m_cullface; } + + /** @name Raw access to triangle shader buffer data + * @{ */ + [[nodiscard]] const auto& shaderTrianglesBuffer() const { return m_triangles; } + [[nodiscard]] auto& shaderTrianglesBufferMutex() const { return m_trianglesMtx; } + /** @} */ + + protected: + void params_serialize(mrpt::serialization::CArchive& out) const; + void params_deserialize(mrpt::serialization::CArchive& in); + + /** List of triangles \sa TTriangle */ + mutable std::vector m_triangles; + mutable mrpt::containers::NonCopiableData m_trianglesMtx; + + /** Returns the bounding box of m_triangles, or (0,0,0)-(0,0,0) if empty. */ + [[nodiscard]] const mrpt::math::TBoundingBoxf trianglesBoundingBox() const; + + private: + bool m_enableLight = true; + TCullFace m_cullface = TCullFace::NONE; +}; + +class VisualObjectParams_TexturedTriangles : public virtual CVisualObject +{ + public: + VisualObjectParams_TexturedTriangles() = default; + virtual ~VisualObjectParams_TexturedTriangles() = default; + + /** Assigns a texture and a transparency image, and enables transparency (If + * the images are not 2^N x 2^M, they will be internally filled to its + * dimensions to be powers of two) + * \note Images are copied, the original ones can be deleted. + */ + void assignImage(const mrpt::img::CImage& img, const mrpt::img::CImage& imgAlpha); + + /** Assigns a texture image, and disable transparency. + * \note Images are copied, the original ones can be deleted. */ + void assignImage(const mrpt::img::CImage& img); + + /** Similar to assignImage, but the passed images are moved in (move + * semantic). */ + void assignImage(mrpt::img::CImage&& img, mrpt::img::CImage&& imgAlpha); + + /** Similar to assignImage, but with move semantics. */ + void assignImage(mrpt::img::CImage&& img); + + [[nodiscard]] bool isLightEnabled() const { return m_enableLight; } + void enableLight(bool enable = true) + { + m_enableLight = enable; + CVisualObject::notifyChange(); + } + + /** Control whether to render the FRONT, BACK, or BOTH (default) set of + * faces. Refer to docs for glCullFace(). + * Example: If set to `cullFaces(TCullFace::BACK);`, back faces will not be + * drawn ("culled") + */ + void cullFaces(const TCullFace& cf) + { + m_cullface = cf; + CVisualObject::notifyChange(); + } + [[nodiscard]] TCullFace cullFaces() const { return m_cullface; } + + [[nodiscard]] const mrpt::img::CImage& getTextureImage() const { return m_textureImage; } + + [[nodiscard]] const mrpt::img::CImage& getTextureAlphaImage() const + { + return m_textureImageAlpha; + } + + [[nodiscard]] bool textureImageHasBeenAssigned() const { return m_textureImageAssigned; } + + /** Enable linear interpolation of textures (default=false, use nearest + * pixel) */ + void enableTextureLinearInterpolation(bool enable) { m_textureInterpolate = enable; } + [[nodiscard]] bool textureLinearInterpolation() const { return m_textureInterpolate; } + + void enableTextureMipMap(bool enable) { m_textureUseMipMaps = enable; } + [[nodiscard]] bool textureMipMap() const { return m_textureUseMipMaps; } + + /** @name Raw access to textured-triangle shader buffer data + * @{ */ + [[nodiscard]] const auto& shaderTexturedTrianglesBuffer() const { return m_triangles; } + [[nodiscard]] auto& shaderTexturedTrianglesBufferMutex() const { return m_trianglesMtx; } + /** @} */ + + protected: + void params_serialize(mrpt::serialization::CArchive& out) const; + void params_deserialize(mrpt::serialization::CArchive& in); + + /** List of triangles \sa TTriangle */ + mutable std::vector m_triangles; + mutable mrpt::containers::NonCopiableData m_trianglesMtx; + + /** Returns the bounding box of m_triangles, or (0,0,0)-(0,0,0) if empty. */ + [[nodiscard]] const mrpt::math::TBoundingBoxf trianglesBoundingBox() const; + + void writeToStreamTexturedObject(mrpt::serialization::CArchive& out) const; + void readFromStreamTexturedObject(mrpt::serialization::CArchive& in); + + private: + bool m_enableLight = true; + TCullFace m_cullface = TCullFace::NONE; + + bool m_textureImageAssigned = false; + mutable mrpt::img::CImage m_textureImage{4, 4}; + mutable mrpt::img::CImage m_textureImageAlpha; + + /** Of the texture using "m_textureImageAlpha" */ + mutable bool m_enableTransparency{false}; + bool m_textureInterpolate = false; + bool m_textureUseMipMaps = true; +}; + +class VisualObjectParams_Lines : public virtual CVisualObject +{ + public: + VisualObjectParams_Lines() = default; + virtual ~VisualObjectParams_Lines() = default; + + void setLineWidth(float w) + { + m_lineWidth = w; + CVisualObject::notifyChange(); + } + [[nodiscard]] float getLineWidth() const { return m_lineWidth; } + void enableAntiAliasing(bool enable = true) + { + m_antiAliasing = enable; + CVisualObject::notifyChange(); + } + [[nodiscard]] bool isAntiAliasingEnabled() const { return m_antiAliasing; } + + protected: + void params_serialize(mrpt::serialization::CArchive& out) const; + void params_deserialize(mrpt::serialization::CArchive& in); + + private: + float m_lineWidth = 1.0f; + bool m_antiAliasing = false; +}; + +class VisualObjectParams_Points : public virtual CVisualObject +{ + public: + VisualObjectParams_Points() = default; + virtual ~VisualObjectParams_Points() = default; + + /** By default is 1.0. \sa enableVariablePointSize() */ + void setPointSize(float p) { m_pointSize = p; } + [[nodiscard]] float getPointSize() const { return m_pointSize; } + + /** Enable/disable variable eye distance-dependent point size (default=true) + */ + void enableVariablePointSize(bool enable = true) { m_variablePointSize = enable; } + [[nodiscard]] bool isEnabledVariablePointSize() const { return m_variablePointSize; } + + /** see CRenderizableShaderPoints for a discussion of this parameter. */ + void setVariablePointSize_k(float v) { m_variablePointSize_K = v; } + [[nodiscard]] float getVariablePointSize_k() const { return m_variablePointSize_K; } + + /** see CRenderizableShaderPoints for a discussion of this parameter. */ + void setVariablePointSize_DepthScale(float v) { m_variablePointSize_DepthScale = v; } + [[nodiscard]] float getVariablePointSize_DepthScale() const + { + return m_variablePointSize_DepthScale; + } + + /** @name Raw access to point shader buffer data + * @{ */ + const auto& shaderPointsVertexPointBuffer() const { return m_vertex_buffer_data; } + const auto& shaderPointsVertexColorBuffer() const { return m_color_buffer_data; } + auto& shaderPointsBuffersMutex() const { return m_pointsMtx; } + + /** @} */ + + protected: + void params_serialize(mrpt::serialization::CArchive& out) const; + void params_deserialize(mrpt::serialization::CArchive& in); + + mutable std::vector m_vertex_buffer_data; + mutable std::vector m_color_buffer_data; + mutable mrpt::containers::NonCopiableData m_pointsMtx; + + /** Returns the bounding box of m_vertex_buffer_data, or (0,0,0)-(0,0,0) if + * empty. */ + const mrpt::math::TBoundingBoxf verticesBoundingBox() const; + + private: + float m_pointSize = 1.0f; + bool m_variablePointSize = true; + float m_variablePointSize_K = 0.1f; + float m_variablePointSize_DepthScale = 0.1f; +}; + +} // namespace mrpt::viz + +MRPT_ENUM_TYPE_BEGIN(mrpt::viz::TCullFace) +using namespace mrpt::viz; +MRPT_FILL_ENUM_MEMBER(TCullFace, NONE); +MRPT_FILL_ENUM_MEMBER(TCullFace, BACK); +MRPT_FILL_ENUM_MEMBER(TCullFace, FRONT); +MRPT_ENUM_TYPE_END() diff --git a/libs/opengl/include/mrpt/opengl/PLY_import_export.h b/libs/viz/include/mrpt/viz/PLY_import_export.h similarity index 98% rename from libs/opengl/include/mrpt/opengl/PLY_import_export.h rename to libs/viz/include/mrpt/viz/PLY_import_export.h index b65f34126c..fb28a5d573 100644 --- a/libs/opengl/include/mrpt/opengl/PLY_import_export.h +++ b/libs/viz/include/mrpt/viz/PLY_import_export.h @@ -14,13 +14,13 @@ #include #include -namespace mrpt::opengl +namespace mrpt::viz { /** A virtual base class that implements the capability of importing 3D point * clouds and faces from a file in the Stanford PLY format. * \sa https://www.mrpt.org/Support_for_the_Stanford_3D_models_file_format_PLY * \sa PLY_Exporter - * \ingroup mrpt_opengl_grp + * \ingroup mrpt_viz_grp */ class PLY_Importer { @@ -131,4 +131,4 @@ class PLY_Exporter }; // End of class def. -} // namespace mrpt::opengl +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/Scene.h b/libs/viz/include/mrpt/viz/Scene.h new file mode 100644 index 0000000000..f9bb4b8bd9 --- /dev/null +++ b/libs/viz/include/mrpt/viz/Scene.h @@ -0,0 +1,250 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include + +/** The namespace for 3D scene representation and rendering. See also the summary page of the mrpt-opengl library for + * more info and thumbnails of many of the render primitive. + */ +namespace mrpt::viz +{ +/** This class allows the user to create, load, save, and render 3D scenes using + * OpenGL primitives. + * The class can be understood as a program to be run over OpenGL, containing + * a sequence of viewport definitions, + * rendering primitives, etc. + * + * It can contain from 1 up to any number of Viewports, each one + * associated a set of OpenGL objects and, optionally, a preferred camera + * position. Both orthogonal (2D/3D) and projection + * camera models can be used for each viewport independently, greatly + * increasing the possibilities of rendered scenes. + * + * An object of Scene always contains at least one viewport + * (mrpt::viz::Viewport), named "main". Optionally, any + * number of other viewports may exist. Viewports are referenced by their + * names, case-sensitive strings. Each viewport contains + * a different 3D scene (i.e. they render different objects), though a + * mechanism exist to share the same 3D scene by a number of + * viewports so memory is not wasted replicating the same objects (see + * Viewport::setCloneView ). + * + * The main rendering method, Scene::render(), assumes a viewport has + * been set-up for the entire target window. That + * method will internally make the required calls to opengl for creating the + * additional viewports. Note that only the depth + * buffer is cleared by default for each (non-main) viewport, to allow + * transparencies. This can be disabled by the approppriate + * member in Viewport. + * + * An object Scene can be saved to a [".3Dscene" + * file](robotics_file_formats.html) using CFileOutputStream or with the direct + * method Scene::saveToFile() for posterior visualization from the + * standalone application \ref app_SceneViewer3D. + * + * It can be also displayed in real-time using windows in mrpt::gui or + * serialized over a network socket, etc. + * + * \ingroup mrpt_viz_grp + */ +class Scene : public mrpt::serialization::CSerializable +{ + DEFINE_SERIALIZABLE(Scene, mrpt::viz) + public: + using TListViewports = std::vector; + + Scene(); + ~Scene() override; + Scene& operator=(const Scene& obj); + Scene(const Scene& obj); + + /** + * Inserts a set of objects into the scene, in the given viewport ("main" + * by default). Any iterable object will be accepted. + * \sa createViewport,getViewport + */ + template + void insertCollection(const T& objs, const std::string& vpn = std::string("main")) + { + insert(objs.begin(), objs.end(), vpn); + } + /** Insert a new object into the scene, in the given viewport (by default, + * into the "main" viewport). + * The viewport must be created previously, an exception will be raised if + * the given name does not correspond to + * an existing viewport. + * \sa createViewport, getViewport + */ + void insert( + const CVisualObject::Ptr& newObject, const std::string& viewportName = std::string("main")); + + /** + * Inserts a set of objects into the scene, in the given viewport ("main" + * by default). + * \sa createViewport,getViewport + */ + template + void insert(const T_it& begin, const T_it& end, const std::string& vpn = std::string("main")) + { + for (T_it it = begin; it != end; it++) insert(*it, vpn); + } + + /** Creates a new viewport, adding it to the scene and returning a pointer + * to the new object. Names (case-sensitive) cannot be duplicated: if the + * name provided coincides with an already existing viewport, a pointer to + * the existing object will be returned. The first, default viewport, is + * named "main". + */ + Viewport::Ptr createViewport(const std::string& viewportName); + + /** Returns the viewport with the given name, or nullptr if it does not + * exist; note that the default viewport is named "main" and initially + * occupies the entire rendering area. + */ + Viewport::Ptr getViewport(const std::string& viewportName = std::string("main")) const; + + const TListViewports& viewports() const { return m_viewports; } + + /** Render this scene */ + void render() const; + + size_t viewportsCount() const { return m_viewports.size(); } + /** Clear the list of objects and viewports in the scene, deleting objects' + * memory, and leaving just the default viewport with the default values. + */ + void clear(bool createMainViewport = true); + + /** If disabled (default), the SceneViewer application will ignore the + * camera of the "main" viewport and keep the viewport selected by the user + * by hand; otherwise, the camera in the "main" viewport prevails. + * \sa followCamera + */ + void enableFollowCamera(bool enabled) { m_followCamera = enabled; } + /** Return the value of "followCamera" + * \sa enableFollowCamera + */ + bool followCamera() const { return m_followCamera; } + /** Returns the first object with a given name, or nullptr (an empty smart + * pointer) if not found. + */ + CVisualObject::Ptr getByName( + const std::string& str, const std::string& viewportName = std::string("main")); + + /** Returns the i'th object of a given class (or of a descendant class), or + nullptr (an empty smart pointer) if not found. + * Example: + * \code + CSphere::Ptr obs = myscene.getByClass(); + * \endcode + * By default (ith=0), the first observation is returned. + */ + template + typename T::Ptr getByClass(size_t ith = 0) const + { + MRPT_START + for (const auto& m_viewport : m_viewports) + { + typename T::Ptr o = m_viewport->getByClass(ith); + if (o) return o; + } + return typename T::Ptr(); // Not found: return empty smart pointer + MRPT_END + } + + /** Removes the given object from the scene (it also deletes the object to + * free its memory). + */ + void removeObject( + const CVisualObject::Ptr& obj, const std::string& viewportName = std::string("main")); + + /** Initializes all textures in the scene (See + * opengl::CTexturedPlane::initializeTextures) + */ + void initializeTextures(); + + /** Retrieves a list of all objects in text form. + * \deprecated Prefer asYAML() (since MRPT 2.1.3) */ + void dumpListOfObjects(std::vector& lst) const; + + /** Prints all viewports and objects in human-readable YAML form. + * Note that not all objects data is serialized, so this method is not + * suitable for deserialization (for that, use saveToFile(), loadFromFile() + * instead). + * \note (New in MRPT 2.1.3) */ + mrpt::containers::yaml asYAML() const; + + /** Saves the scene to a [".3Dscene" file](robotics_file_formats.html), + * loadable by: \ref app_SceneViewer3D + * \sa loadFromFile + * \return false on any error. + */ + bool saveToFile(const std::string& fil) const; + + /** Loads the scene from a [".3Dscene" file](robotics_file_formats.html). + * \sa saveToFile + * \return false on any error. + */ + bool loadFromFile(const std::string& fil); + + /** Traces a ray + */ + bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const; + + /** Evaluates the bounding box of the scene in the given viewport (default: + * "main"). */ + mrpt::math::TBoundingBox getBoundingBox(const std::string& vpn = std::string("main")) const; + + /** Recursive depth-first visit all objects in all viewports of the scene, + * calling the user-supplied function + * The passed function must accept only one argument of type "const + * mrpt::viz::CVisualObject::Ptr &" + */ + template + void visitAllObjects(FUNCTOR functor) const + { + MRPT_START + for (const auto& viewport : m_viewports) + for (auto& o : *viewport) internal_visitAllObjects(functor, o); + MRPT_END + } + + protected: + bool m_followCamera = false; + + /** The list of viewports, indexed by name. */ + TListViewports m_viewports; + + template + static void internal_visitAllObjects(FUNCTOR functor, const CVisualObject::Ptr& o) + { + functor(o); + if (auto objs = std::dynamic_pointer_cast(o); objs) + { + for (auto& obj : *objs) internal_visitAllObjects(functor, obj); + } + } +}; + +/** Inserts an openGL object into a scene. Allows call chaining. \sa + * mrpt::viz::Scene::insert */ +Scene::Ptr& operator<<(Scene::Ptr& s, const CVisualObject::Ptr& r); + +/**Inserts any iterable collection of openGL objects into a scene, allowing call + * chaining. \sa mrpt::viz::Scene::insert */ +template +Scene::Ptr& operator<<(Scene::Ptr& s, const std::vector& v) +{ + s->insert(v.begin(), v.end()); + return s; +} +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/TLightParameters.h b/libs/viz/include/mrpt/viz/TLightParameters.h new file mode 100644 index 0000000000..0d48afaba6 --- /dev/null +++ b/libs/viz/include/mrpt/viz/TLightParameters.h @@ -0,0 +1,67 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include +#include + +namespace mrpt::viz +{ +/** Unidirectional lighting model parameters for triangle shaders. + * Refer to standard OpenGL literature and tutorials for the meaning of each + * field, and to the shader GLSL code itself. + * \ingroup mrpt_viz_grp + */ +struct TLightParameters +{ + TLightParameters() = default; + ~TLightParameters() = default; + + mrpt::img::TColorf color = {1.0f, 1.0f, 1.0f}; + + float diffuse = 0.8f; + float ambient = 0.2f; + float specular = 0.95f; + + /** Light direction (must be normalized) */ + mrpt::math::TVector3Df direction = {-0.40825f, -0.40825f, -0.81650f}; + + /** Shadow tuning parameters ("anti shadow acne") */ + float shadow_bias = 1e-5, shadow_bias_cam2frag = 1e-5, shadow_bias_normal = 1e-4; + + /** Multiplier from eye distance to the length size of the squared area in + * which to evaluate shadow casting by unidirectional light. + * Unitless (meter/meter). + * \note (New in MRPT 2.10.0) + */ + double eyeDistance2lightShadowExtension = 2.0; + + /** Minimum extension (in [0,1] ratio of the light distance) of the shadow + * map square ortho frustum. Should be roughly the maximum area of the + * largest room for indoor environments to ensure no missing shadows in + * distant areas. + * + * \note (New in MRPT 2.10.0) + */ + float minimum_shadow_map_extension_ratio = 0.03f; + + void writeToStream(mrpt::serialization::CArchive& out) const; + void readFromStream(mrpt::serialization::CArchive& in); + + DECLARE_TTYPENAME_CLASSNAME(mrpt::viz::TLightParameters) +}; + +mrpt::serialization::CArchive& operator>>( + mrpt::serialization::CArchive& in, mrpt::viz::TLightParameters& o); +mrpt::serialization::CArchive& operator<<( + mrpt::serialization::CArchive& out, const mrpt::viz::TLightParameters& o); + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/TTriangle.h b/libs/viz/include/mrpt/viz/TTriangle.h new file mode 100644 index 0000000000..6305f54e5f --- /dev/null +++ b/libs/viz/include/mrpt/viz/TTriangle.h @@ -0,0 +1,146 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace mrpt::viz +{ +// Ensure 1-byte memory alignment, no additional stride bytes. +#pragma pack(push, 1) + +/** A triangle (float coordinates) with RGBA colors (u8) and UV (texture + * coordinates) for each vertex. Note that not all the fields must be filled in, + * it depends on the consumer of the structure. + * + * The structure is memory packed to 1-byte, to ensure it can be used in GPU + * memory vertex arrays without unexpected paddings. + * + * \sa opengl::Scene, CSetOfTexturedTriangles + * \ingroup mrpt_viz_grp + */ +struct TTriangle +{ + struct Vertex + { + mrpt::math::TPointXYZfRGBAu8 xyzrgba; + mrpt::math::TVector3Df normal; //!< Must not be normalized + mrpt::math::TVector2Df uv; //!< texture coordinates (0,0)-(1,1) + + void setColor(const mrpt::img::TColor& c) + { + xyzrgba.r = c.R; + xyzrgba.g = c.G; + xyzrgba.b = c.B; + xyzrgba.a = c.A; + } + }; + + TTriangle() = default; + explicit TTriangle(const mrpt::math::TPolygon3D& p) + { + ASSERT_EQUAL_(p.size(), 3U); + for (size_t i = 0; i < 3; i++) + { + auto& pp = vertices[i].xyzrgba; + pp.pt = p[i].cast(); + pp.r = pp.g = pp.b = 0xff; + } + computeNormals(); + } + /** Constructor from 3 points (default normals are computed) */ + explicit TTriangle( + const mrpt::math::TPoint3Df& p1, + const mrpt::math::TPoint3Df& p2, + const mrpt::math::TPoint3Df& p3) + { + vertices[0].xyzrgba.pt = p1; + vertices[1].xyzrgba.pt = p2; + vertices[2].xyzrgba.pt = p3; + computeNormals(); + } + /** Constructor from 3 points and its 3 normals */ + explicit TTriangle( + const mrpt::math::TPoint3Df& p1, + const mrpt::math::TPoint3Df& p2, + const mrpt::math::TPoint3Df& p3, + const mrpt::math::TVector3Df& n1, + const mrpt::math::TVector3Df& n2, + const mrpt::math::TVector3Df& n3) + { + vertices[0].xyzrgba.pt = p1; + vertices[0].normal = n1; + vertices[1].xyzrgba.pt = p2; + vertices[1].normal = n2; + vertices[2].xyzrgba.pt = p3; + vertices[2].normal = n3; + } + + std::array vertices; + + const float& x(size_t i) const { return vertices[i].xyzrgba.pt.x; } + const float& y(size_t i) const { return vertices[i].xyzrgba.pt.y; } + const float& z(size_t i) const { return vertices[i].xyzrgba.pt.z; } + const uint8_t& r(size_t i) const { return vertices[i].xyzrgba.r; } + const uint8_t& g(size_t i) const { return vertices[i].xyzrgba.g; } + const uint8_t& b(size_t i) const { return vertices[i].xyzrgba.b; } + const uint8_t& a(size_t i) const { return vertices[i].xyzrgba.a; } + const float& u(size_t i) const { return vertices[i].uv.x; } + const float& v(size_t i) const { return vertices[i].uv.y; } + float& x(size_t i) { return vertices[i].xyzrgba.pt.x; } + float& y(size_t i) { return vertices[i].xyzrgba.pt.y; } + float& z(size_t i) { return vertices[i].xyzrgba.pt.z; } + uint8_t& r(size_t i) { return vertices[i].xyzrgba.r; } + uint8_t& g(size_t i) { return vertices[i].xyzrgba.g; } + uint8_t& b(size_t i) { return vertices[i].xyzrgba.b; } + uint8_t& a(size_t i) { return vertices[i].xyzrgba.a; } + float& u(size_t i) { return vertices[i].uv.x; } + float& v(size_t i) { return vertices[i].uv.y; } + + mrpt::math::TPoint3Df& vertex(size_t i) { return vertices[i].xyzrgba.pt; } + const mrpt::math::TPoint3Df& vertex(size_t i) const { return vertices[i].xyzrgba.pt; } + + /** Sets the color of all vertices */ + void setColor(const mrpt::img::TColor& c) + { + for (size_t i = 0; i < 3; i++) + { + vertices[i].xyzrgba.r = c.R; + vertices[i].xyzrgba.g = c.G; + vertices[i].xyzrgba.b = c.B; + vertices[i].xyzrgba.a = c.A; + } + } + void setColor(const mrpt::img::TColorf& c) + { + setColor(mrpt::img::TColor(f2u8(c.R), f2u8(c.G), f2u8(c.B), f2u8(c.A))); + } + + /** Compute the three normals from the cross-product of "v01 x v02". + * Note that using this default normals will not lead to interpolated + * lighting in the fragment shaders, since all vertex are equal; a derived + * class should use custom, more accurate normals to enable soft lighting. + */ + void computeNormals(); + + // These methods are explicitly instantiated for T=float and double. + void readFrom(mrpt::serialization::CArchive& i); + void writeTo(mrpt::serialization::CArchive& o) const; +}; +#pragma pack(pop) + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/Viewport.h b/libs/viz/include/mrpt/viz/Viewport.h new file mode 100644 index 0000000000..6053322640 --- /dev/null +++ b/libs/viz/include/mrpt/viz/Viewport.h @@ -0,0 +1,422 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mrpt::img +{ +class CImage; +} +namespace mrpt::viz +{ +/** A viewport within a Scene, containing a set of OpenGL objects to + *render. + * This class has protected constructor, thus it cannot be created by users. + *Use Scene::createViewport instead. + * A viewport has these "operation modes": + * - Normal (default): It renders the contained objects. + * - Cloned: It clones the objects from another viewport. See \a + *setCloneView() + * - Image mode: It renders an image (e.g. from a video stream) efficiently + *using a textued quad. See \a setImageView(). + * + * In any case, the viewport can be resized to only fit a part of the entire + *parent viewport. + * There will be always at least one viewport in a Scene named "main". + * + * This class can be observed (see mrpt::system::CObserver) for the following + *events (see mrpt::system::mrptEvent): + * - mrpt::viz::mrptEventGLPreRender + * - mrpt::viz::mrptEventGLPostRender + * + * Two directional light sources at infinity are created by default, with + *directions (-1,-1,-1) and (1,2,1), respectively. + * + * Lighting parameters are accessible via lightParameters(). + * + * Refer to mrpt::viz::Scene for further details. + * \ingroup mrpt_viz_grp + */ +class Viewport : + public mrpt::serialization::CSerializable, + public mrpt::system::CObservable, + public mrpt::viz::CTextMessageCapable +{ + DEFINE_SERIALIZABLE(Viewport, mrpt::viz) + friend class Scene; + + public: + /** @name Viewport "modes" + @{ */ + + /** Set this viewport as a clone of some other viewport, given its name - as + * a side effect, current list of internal OpenGL objects is cleared. + * By default, only the objects are cloned, not the camera. See + * \sa resetCloneView + */ + void setCloneView(const std::string& clonedViewport); + + /** Set this viewport into "image view"-mode, where an image is efficiently + * drawn (fitting the viewport area) using an OpenGL textured quad. + * Call this method with the new image to update the displayed image (but + * recall to first lock the parent openglscene's critical section, then do + * the update, then release the lock, and then issue a window repaint). + * Internally, the texture is drawn using a mrpt::viz::CTexturedPlane + * The viewport can be reverted to behave like a normal viewport by + * calling setNormalMode() + * + * \param[in] transparentBackground This method can also make the viewport + * transparent (default), so the area not filled with the image still allows + * seeing an underlying viewport. + */ + void setImageView(const mrpt::img::CImage& img, bool transparentBackground = true); + + /** \overload With move semantics */ + void setImageView(mrpt::img::CImage&& img, bool transparentBackground = true); + + /** Returns true if setImageView() has been called on this viewport */ + [[nodiscard]] bool isImageViewMode() const { return !!m_imageViewPlane; } + + /** Reset the viewport to normal mode: rendering its own objects. + * \sa setCloneView, setNormalMode + */ + void resetCloneView() { setNormalMode(); } + + /** If set to true, and setCloneView() has been called, this viewport will + * be rendered using the camera of the cloned viewport. + */ + void setCloneCamera(bool enable); + + /** Use the camera of another viewport. + * Note this works even for viewports not in "clone" mode, so you can + * render different scenes but using the same camera. + */ + void setClonedCameraFrom(const std::string& viewPortName) + { + m_isClonedCamera = true; + m_clonedCameraViewport = viewPortName; + } + + /** Resets the viewport to a normal 3D viewport \sa setCloneView, + * setImageView */ + void setNormalMode(); + + void setViewportVisibility(bool visible) { m_isViewportVisible = visible; } + [[nodiscard]] bool getViewportVisibility() const { return m_isViewportVisible; } + + /** @} */ + // end of Set the "viewport mode" + + /** @name OpenGL global settings that affect rendering all objects in the + scene/viewport + @{ */ + + /** Sets glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST) is enabled, or GL_FASTEST + * otherwise. */ + void enablePolygonNicest(bool enable = true) { m_OpenGL_enablePolygonNicest = enable; } + [[nodiscard]] bool isPolygonNicestEnabled() const { return m_OpenGL_enablePolygonNicest; } + + [[nodiscard]] const TLightParameters& lightParameters() const { return m_light; } + [[nodiscard]] TLightParameters& lightParameters() { return m_light; } + + /** @} */ + + /** @name Change or read viewport properties (except "viewport modes") + @{ */ + + /** Returns the name of the viewport */ + [[nodiscard]] std::string getName() { return m_name; } + + /** Change the viewport position and dimension on the rendering window. + * X & Y coordinates here can have two interpretations: + * - If in the range [0,1], they are factors with respect to the actual + *window sizes (i.e. width=1 means the entire width of the rendering + *window). + * - If >1, they are interpreted as pixels. + * + * width & height can be interpreted as: + * - If >1, they are the size of the viewport in that dimension, in + *pixels. + * - If in [0,1], they are the size of the viewport in that dimension, + *in + *a factor of the width/height. + * - If in [-1,0[, the size is computed such as the right/top border + *ends + *up in the given coordinate, interpreted as a factor (e.g. -1: up to the + *end of the viewport, -0.5: up to the middle of it). + * - If <-1 the size is computed such as the right/top border ends up + *in + *the given absolute coordinate (e.g. -200: up to the row/column 200px). + * + * \note (x,y) specify the lower left corner of the viewport rectangle. + * \sa getViewportPosition + */ + void setViewportPosition(const double x, const double y, const double width, const double height); + + /** Get the current viewport position and dimension on the rendering window. + * X & Y coordinates here can have two interpretations: + * - If in the range [0,1], they are factors with respect to the actual + * window sizes (i.e. width=1 means the entire width of the rendering + * window). + * - If >1, they are interpreted as pixels. + * \note (x,y) specify the lower left corner of the viewport rectangle. + * \sa setViewportPosition + */ + void getViewportPosition(double& x, double& y, double& width, double& height); + + /** Set the min/max clip depth distances of the rendering frustum (default: + * 0.1 - 1000) + * \sa getViewportClipDistances + */ + void setViewportClipDistances(const float clip_min, const float clip_max); + + /** Get the current min/max clip depth distances of the rendering frustum + * (default: 0.1 - 1000) + * \sa setViewportClipDistances + */ + void getViewportClipDistances(float& clip_min, float& clip_max) const; + + void setLightShadowClipDistances(const float clip_min, const float clip_max); + void getLightShadowClipDistances(float& clip_min, float& clip_max) const; + + /** Set the border size ("frame") of the viewport (default=0) */ + void setBorderSize(unsigned int lineWidth) { m_borderWidth = lineWidth; } + [[nodiscard]] unsigned int getBorderSize() const { return m_borderWidth; } + + void setBorderColor(const mrpt::img::TColor& c) { m_borderColor = c; } + [[nodiscard]] const mrpt::img::TColor& getBorderColor() const { return m_borderColor; } + + /** Return whether the viewport will be rendered transparent over previous + * viewports. + */ + [[nodiscard]] bool isTransparent() { return m_isTransparent; } + + /** Set the transparency, that is, whether the viewport will be rendered + * transparent over previous viewports (default=false). + */ + void setTransparent(bool trans) { m_isTransparent = trans; } + + /** Defines the viewport background color */ + void setCustomBackgroundColor(const mrpt::img::TColorf& color) { m_background_color = color; } + + [[nodiscard]] mrpt::img::TColorf getCustomBackgroundColor() const { return m_background_color; } + + /** Enables or disables rendering of shadows cast by the unidirectional + * light. + * \param enabled Set to true to enable shadow casting + * (default at ctor=false). + * \param SHADOW_MAP_SIZE_X Width of the shadow cast map (1st pass of + * rendering with shadows). Larger values are slower but gives + * more precise shadows. Default=2048x2048. + * Zero means do not change. + * \param SHADOW_MAP_SIZE_Y Like SHADOW_MAP_SIZE_X but defines the height. + * + */ + void enableShadowCasting( + bool enabled = true, unsigned int SHADOW_MAP_SIZE_X = 0, unsigned int SHADOW_MAP_SIZE_Y = 0); + + [[nodiscard]] bool isShadowCastingEnabled() const { return m_shadowsEnabled; } + + /** @} */ // end of Change or read viewport properties + + /** @name Contained objects set/get/search + @{ */ + + using const_iterator = ListVisualObjects::const_iterator; + using iterator = ListVisualObjects::iterator; + + [[nodiscard]] const_iterator begin() const { return m_objects.begin(); } + [[nodiscard]] const_iterator end() const { return m_objects.end(); } + [[nodiscard]] iterator begin() { return m_objects.begin(); } + [[nodiscard]] iterator end() { return m_objects.end(); } + + /** Delete all internal obejcts * \sa insert */ + void clear(); + + /** Insert a new object into the list. + * The object MUST NOT be deleted, it will be deleted automatically by + * this object when not required anymore. + */ + void insert(const CVisualObject::Ptr& newObject); + + /** Changes the point of view of the camera, from a given pose. + * \sa getCurrentCameraPose + */ + void setCurrentCameraFromPose(mrpt::poses::CPose3D& p); + + /** Returns the first object with a given name, or nullptr if not found. + */ + [[nodiscard]] CVisualObject::Ptr getByName(const std::string& str); + + /** Returns the i'th object of a given class (or of a descendant class), or + nullptr (an empty smart pointer) if not found. + * Example: + * \code + CSphere::Ptr obs = view.getByClass(); + * \endcode + * By default (ith=0), the first observation is returned. + */ + template + [[nodiscard]] typename T::Ptr getByClass(size_t ith = 0) const + { + MRPT_START + size_t foundCount = 0; + for (const auto& o : m_objects) + if (const auto f = std::dynamic_pointer_cast(o); f) + { + if (foundCount++ == ith) return f; + } + + // If not found directly, search recursively: + for (const auto& o : m_objects) + { + if (auto obj = std::dynamic_pointer_cast(o); obj) + { + if (auto f = obj->template getByClass(ith); f) return f; + } + } + return typename T::Ptr(); // Not found: return empty smart pointer + MRPT_END + } + + /** Removes the given object from the scene (it also deletes the object to + * free its memory). + */ + void removeObject(const CVisualObject::Ptr& obj); + + /** Number of objects contained. */ + [[nodiscard]] size_t size() const { return m_objects.size(); } + + [[nodiscard]] bool empty() const { return m_objects.empty(); } + + /** Get a reference to the camera associated with this viewport. */ + [[nodiscard]] mrpt::viz::CCamera& getCamera() { return m_camera; } + + /** Get a reference to the camera associated with this viewport. */ + [[nodiscard]] const mrpt::viz::CCamera& getCamera() const { return m_camera; } + + mrpt::math::TBoundingBox getBoundingBox() const; + + /** @} */ // end of Contained objects set/get/search + + /** Destructor: clears all objects. */ + ~Viewport() override; + + /** Constructor, invoked from Scene only. + */ + Viewport(Scene* parent = nullptr, const std::string& name = std::string("")); + + /** Render the objects in this viewport (called from Scene) */ + void render( + const int render_width, + const int render_height, + const int render_offset_x = 0, + const int render_offset_y = 0, + const CCamera* forceThisCamera = nullptr) const; + + protected: + /** Retrieves a list of all objects in text form. + * \deprecated Prefer asYAML() (since MRPT 2.1.3) */ + void dumpListOfObjects(std::vector& lst) const; + + /** Prints all viewport objects in human-readable YAML form. + * \note (New in MRPT 2.1.3) */ + [[nodiscard]] mrpt::containers::yaml asYAML() const; + + /** The camera associated to the viewport */ + mrpt::viz::CCamera m_camera; + + /** The scene that contains this viewport. */ + mrpt::safe_ptr m_parent; + + /** Set by setCloneView */ + bool m_isCloned = false; + + /** Set by setCloneCamera */ + bool m_isClonedCamera = false; + + bool m_isViewportVisible = true; + + /** Only if m_isCloned=true */ + std::string m_clonedViewport; + + /** If m_isClonedCamera && !m_isCloned, take the camera from another view, + * to render a different scene. */ + std::string m_clonedCameraViewport; + + /** The viewport's name */ + std::string m_name; + + /** Whether to clear color buffer. */ + bool m_isTransparent = false; + + /** Default=0, the border around the viewport. */ + uint32_t m_borderWidth = 0; + + mrpt::img::TColor m_borderColor{255, 255, 255, 255}; + + /** The viewport position [0,1] */ + double m_view_x{0}, m_view_y{0}, m_view_width{1}, m_view_height{1}; + + /** The min/max clip depth distances (default: 0.01 - 1000) */ + float m_clip_min = 0.01f, m_clip_max = 1000.0f; + + /** The near/far plane clip distances for unidirectional light shadow + * casting */ + float m_lightShadowClipMin = 0.01f, m_lightShadowClipMax = 1000.0f; + + mrpt::img::TColorf m_background_color = {0.4f, 0.4f, 0.4f}; + + /** The image to display, after calling \a setImageView() */ + mrpt::viz::CTexturedPlane::Ptr m_imageViewPlane; + + mutable mrpt::viz::CSetOfLines::Ptr m_borderLines; + + const CCamera* internalResolveActiveCamera(const CCamera* forceThisCamera = nullptr) const; + + mrpt::viz::ListVisualObjects m_objects; + + void internal_enableImageView(bool transparentBackground); + + // OpenGL global settings: + bool m_OpenGL_enablePolygonNicest{true}; + + TLightParameters m_light; + + bool m_shadowsEnabled = false; + uint32_t m_ShadowMapSizeX = 2048, m_ShadowMapSizeY = 2048; +}; + +/** + * Inserts an openGL object into a viewport. Allows call chaining. + * \sa mrpt::viz::Viewport::insert + */ +Viewport::Ptr& operator<<(Viewport::Ptr& s, const CVisualObject::Ptr& r); + +/** + * Inserts any iterable set of openGL objects into a viewport. Allows call + * chaining. + * \sa mrpt::viz::Viewport::insert + */ +Viewport::Ptr& operator<<(Viewport::Ptr& s, const std::vector& v); + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/Visualizable.h b/libs/viz/include/mrpt/viz/Visualizable.h new file mode 100644 index 0000000000..dbd6dac58c --- /dev/null +++ b/libs/viz/include/mrpt/viz/Visualizable.h @@ -0,0 +1,43 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +#include // shared_ptr + +namespace mrpt::viz +{ +/** Interface for classes visualizable as an mrpt::viz::CSetOfObjects. + * + * \ingroup mrpt_viz_grp + */ +class Visualizable +{ + public: + Visualizable() = default; + ~Visualizable() = default; + + /** Inserts 3D primitives representing this object into the provided + * container. + * Note that the former contents of `o` are not cleared. + * + * \sa getVisualization() + */ + virtual void getVisualizationInto(mrpt::viz::CSetOfObjects& o) const = 0; + + /** Creates 3D primitives representing this objects. + * This is equivalent to getVisualizationInto() but creating, and returning + * by value, a new rpt::opengl::CSetOfObjects::Ptr shared pointer. + * \sa getVisualizationInto() + */ + std::shared_ptr getVisualization() const; +}; + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/graph_tools.h b/libs/viz/include/mrpt/viz/graph_tools.h new file mode 100644 index 0000000000..a0130cfcb6 --- /dev/null +++ b/libs/viz/include/mrpt/viz/graph_tools.h @@ -0,0 +1,69 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include + +namespace mrpt +{ +/** \ingroup mrpt_viz_grp */ +namespace opengl +{ +/** Tool functions for graphs of pose constraints. \ingroup mrpt_viz_grp */ +namespace graph_tools +{ +/** @name Tool functions for graphs of pose constraints + @{ */ + +// clang-format off +/** Returns an opengl objects representation of an arbitrary graph, as a network + *of 3D pose frames. + * Note that the "global" coordinates of each node are taken from + *mrpt::graphs::CNetworkOfPoses::nodes, so + * if a node appears in "edges" but not in "nodes" it will be not displayed. + * + * \param g The graph + * \param extra_params An extra set of optional parameters (see below). + * + * List of accepted extra parameters: + * + * + * | Parameter name | Type | Description | Default | + * |---------------------------|---------|-----------------------------------------------------------------------------------------------------------------------------------------------------|------------| + * | show_ID_labels | bool | Show poses ID labels | false | + * | show_ground_grid | bool | Creates a gray grid on the ground level (mrpt::viz::CGridPlaneXY). The extension of the grid is computed to cover the entire graph extension | true | + * | show_edges | bool | Draw lines between nodes with at least one edge between them. | true | + * | show_node_corners | bool | Draw a small 3D corner frame at each node (see mrpt::viz::stock_objects::CornerXYZSimple) | true | + * | show_edge_rel_poses | bool | Draw the relative poses stored in each edge as a small extra 3D corner frame at each "node pose \oplus edge pose" | false | + * | nodes_point_size | double | If set to !=0, draw a point of the given size (glPointSize) at each node. | 0.0 | + * | nodes_corner_scale | double | If show_node_corners==true, the size (length) of the corner lines. | 0.7 | + * | nodes_edges_corner_scale | double | If show_edge_rel_poses==true, the size of the corners at the end of each drawn edge | 0.4 | + * | nodes_point_color | int | If nodes_point_size>0, set this value to a hexadecimal int value 0xRRGGBBAA with the desired RGB+Alpha color of points. | 0xA0A0A0FF | + * | edge_color | int | If show_edges==true, the color of those edges as a hexadecimal int value 0xRRGGBBAA with RGB+Alpha color. | 0x0000FF40 | + * | edge_rel_poses_color | int | If show_edge_rel_poses==true, the color of those edges as a hexadecimal int value 0xRRGGBBAA with RGB+Alpha color. | 0xFF800040 | + * | edge_width | double | If show_edges==true, the width of edge lines. | 1 | + * + * \sa mrpt::graphs::CNetworkOfPoses2D, mrpt::graphs::CNetworkOfPoses3D, + *mrpt::graphs::CNetworkOfPoses2DInf, mrpt::graphs::CNetworkOfPoses3DInf + * \note Implemented as headers-only in \a graph_tools_impl.h + * \ingroup mrpt_viz_grp + */ +// clang-format on +template +CSetOfObjects::Ptr graph_visualize( + const GRAPH_T& g, const mrpt::containers::yaml& extra_params = {}); + +/** @} */ +} // namespace graph_tools +} // namespace opengl + +} // namespace mrpt + +#include "graph_tools_impl.h" diff --git a/libs/viz/include/mrpt/viz/graph_tools_impl.h b/libs/viz/include/mrpt/viz/graph_tools_impl.h new file mode 100644 index 0000000000..b830a4b02a --- /dev/null +++ b/libs/viz/include/mrpt/viz/graph_tools_impl.h @@ -0,0 +1,195 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mrpt::viz::graph_tools +{ +template +CSetOfObjects::Ptr graph_visualize(const GRAPH_T& g, const mrpt::containers::yaml& ep) +{ + MRPT_TRY_START + + using mrpt::math::TPose3D; + using mrpt::poses::CPose3D; + + // Is a 2D or 3D graph network? + using constraint_t = typename GRAPH_T::constraint_t; + + const bool is_3D_graph = constraint_t::is_3D(); + + // create opengl obejct to be filled. + CSetOfObjects::Ptr ret = std::make_shared(); + + // graph visualization parameters + const bool show_ID_labels = ep.getOrDefault("show_ID_labels", false); + const bool show_ground_grid = ep.getOrDefault("show_ground_grid", true); + const bool show_edges = ep.getOrDefault("show_edges", true); + const bool show_node_corners = ep.getOrDefault("show_node_corners", true); + const bool show_edge_rel_poses = ep.getOrDefault("show_edge_rel_poses", false); + const double nodes_point_size = ep.getOrDefault("nodes_point_size", 0.); + const double nodes_corner_scale = ep.getOrDefault("nodes_corner_scale", 0.7); + const double nodes_edges_corner_scale = ep.getOrDefault("nodes_edges_corner_scale", 0.4); + const unsigned int nodes_point_color = + ep.getOrDefault("nodes_point_color", (unsigned int)0xA0A0A0); + const unsigned int edge_color = + ep.getOrDefault("edge_color", (unsigned int)0x0000FF40); + const unsigned int edge_rel_poses_color = + ep.getOrDefault("edge_rel_poses_color", (unsigned int)0xFF800040); + const double edge_width = ep.getOrDefault("edge_width", 2.); + + if (show_ground_grid) + { + // Estimate bounding box. + mrpt::math::TPoint3D BB_min(-10., -10., 0.), BB_max(10., 10., 0.); + + for (auto itNod = g.nodes.begin(); itNod != g.nodes.end(); ++itNod) + { + const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type. + + keep_min(BB_min.x, p.x()); + keep_min(BB_min.y, p.y()); + keep_min(BB_min.z, p.z()); + + keep_max(BB_max.x, p.x()); + keep_max(BB_max.y, p.y()); + keep_max(BB_max.z, p.z()); + } + + // Create ground plane: + const double grid_frequency = 5.0; + CGridPlaneXY::Ptr grid = + CGridPlaneXY::Create(BB_min.x, BB_max.x, BB_min.y, BB_max.y, BB_min.z, grid_frequency); + grid->setColor(0.3f, 0.3f, 0.3f); + ret->insert(grid); + } // end show_ground_grid + + // Draw nodes as thick points: + if (nodes_point_size > 0) + { + CPointCloud::Ptr pnts = std::make_shared(); + pnts->setColor(mrpt::img::TColorf(mrpt::img::TColor(nodes_point_color))); + pnts->setPointSize(nodes_point_size); + + // Add nodes: + for (auto itNod = g.nodes.begin(); itNod != g.nodes.end(); ++itNod) + { + const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type. + pnts->insertPoint(p.x(), p.y(), p.z()); + } + + ret->insert(pnts); + } // end draw node points + + // Show a 2D corner at each node (or just an empty object with the ID label) + if (show_node_corners || show_ID_labels) + { + for (auto itNod = g.nodes.begin(); itNod != g.nodes.end(); ++itNod) + { + const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type. + CSetOfObjects::Ptr gl_corner = + show_node_corners + ? (is_3D_graph + ? stock_objects::CornerXYZSimple(nodes_corner_scale, 1.0 /*line width*/) + : stock_objects::CornerXYSimple(nodes_corner_scale, 1.0 /*line width*/)) + : std::make_shared(); + gl_corner->setPose(p); + if (show_ID_labels) // don't show IDs twice! + { + gl_corner->setName(format("%u", static_cast(itNod->first))); + gl_corner->enableShowName(); + } + ret->insert(gl_corner); + } + } // end draw node corners + + if (show_edge_rel_poses) + { + const mrpt::img::TColor col8bit(edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24); + + for (const auto& edge : g) + { + // Node ID of the source pose: + const auto node_id_start = g.edges_store_inverse_poses ? edge.first.second : edge.first.first; + + // Draw only if we have the global coords of starting nodes: + auto itNod = g.nodes.find(node_id_start); + if (itNod != g.nodes.end()) + { + const CPose3D pSource = CPose3D(itNod->second); + // Create a set of objects at that pose and do the rest in + // relative coords: + auto gl_rel_edge = mrpt::viz::CSetOfObjects::Create(); + gl_rel_edge->setPose(pSource); + + const auto& edge_pose = edge.second.getPoseMean(); + const auto edge_pose_pt = mrpt::poses::CPoint3D(edge_pose); + + auto gl_edge_corner = + (is_3D_graph + ? stock_objects::CornerXYZSimple(nodes_edges_corner_scale, 1.0 /*line width*/) + : stock_objects::CornerXYSimple(nodes_edges_corner_scale, 1.0 /*line width*/)); + + gl_edge_corner->setPose(edge_pose); + gl_rel_edge->insert(gl_edge_corner); + + auto gl_line = mrpt::viz::CSimpleLine::Create( + 0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(), edge_pose_pt.z()); + gl_line->setColor_u8(col8bit); + gl_line->setLineWidth(edge_width); + gl_rel_edge->insert(gl_line); + + ret->insert(gl_rel_edge); + } + } + } + + if (show_edges) + { + CSetOfLines::Ptr gl_edges = std::make_shared(); + const mrpt::img::TColor col8bit(edge_color >> 8, edge_color & 0x000000ff); + + gl_edges->setColor_u8(col8bit); + gl_edges->setLineWidth(edge_width); + + for (const auto& edge : g) + { + const auto id1 = edge.first.first; + const auto id2 = edge.first.second; + + // Draw only if we have the global coords of both nodes: + auto itNod1 = g.nodes.find(id1); + auto itNod2 = g.nodes.find(id2); + if (itNod1 != g.nodes.end() && itNod2 != g.nodes.end()) + { + const CPose3D p1 = CPose3D(itNod1->second); + const CPose3D p2 = CPose3D(itNod2->second); + gl_edges->appendLine( + mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()), + mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z())); + } + } + ret->insert(gl_edges); + + } // end draw edges + + return ret; + + MRPT_TRY_END +} +} // namespace mrpt::viz::graph_tools diff --git a/libs/opengl/include/mrpt/opengl/opengl_fonts.h b/libs/viz/include/mrpt/viz/opengl_fonts.h similarity index 97% rename from libs/opengl/include/mrpt/opengl/opengl_fonts.h rename to libs/viz/include/mrpt/viz/opengl_fonts.h index 63eb23a89b..b26c5feed8 100644 --- a/libs/opengl/include/mrpt/opengl/opengl_fonts.h +++ b/libs/viz/include/mrpt/viz/opengl_fonts.h @@ -14,7 +14,7 @@ #include -namespace mrpt::opengl +namespace mrpt::viz { /** Different style for vectorized font rendering \sa T2DTextData */ enum TOpenGLFontStyle @@ -50,7 +50,7 @@ struct TFontParams mrpt::img::TColorf shadow_color = {0.0f, 0.0f, 0.0f, 1.0f}; /** (default: FILL) See TOpenGLFontStyle. */ - TOpenGLFontStyle vfont_style = opengl::FILL; + TOpenGLFontStyle vfont_style = mrpt::viz::FILL; /** (default: 1.5) Refer to mrpt::opengl::gl_utils::glDrawText */ double vfont_spacing = 1.5; diff --git a/libs/viz/include/mrpt/viz/pointcloud_adapters.h b/libs/viz/include/mrpt/viz/pointcloud_adapters.h new file mode 100644 index 0000000000..587a999855 --- /dev/null +++ b/libs/viz/include/mrpt/viz/pointcloud_adapters.h @@ -0,0 +1,43 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +namespace mrpt::viz +{ +// clang-format off +/** \defgroup mrpt_adapters_grp Pointcloud adapter (wrapper) template classes (in #include ) + * \ingroup mrpt_viz_grp + */ +// clang-format on + +/** \addtogroup mrpt_adapters_grp + * @{ */ + +/** An adapter to different kinds of point cloud object. + * Implemented as a pure C++ template with specializations for the highest + * flexibility and efficiency in compiler-generated implementations. + * Usage: + * \code + * PC my_obj; + * my_obj.specific_methods(); + * // ... + * PointCloudAdapter pca(my_obj); + * pca.unified_interface_methods(); + * // ... + * \endcode + * See specializations for details on the exposed API. + */ +template +class PointCloudAdapter; + +/** @} */ // end of grouping + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/pose_pdfs.h b/libs/viz/include/mrpt/viz/pose_pdfs.h new file mode 100644 index 0000000000..198a1e5d5b --- /dev/null +++ b/libs/viz/include/mrpt/viz/pose_pdfs.h @@ -0,0 +1,27 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +namespace mrpt::viz +{ +/** @name Functions to obtain a 3D representation of a pose PDF + @{ */ + +/** Returns a representation of a the PDF - this is just an auxiliary function, + * it's more natural to call mrpt::poses::CPosePDF::getAs3DObject */ +template +CSetOfObjects::Ptr posePDF2opengl(const POSE_PDF& o) +{ + return CSetOfObjects::posePDF2opengl(o); +} + +/** @} */ +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/registerAllClasses.h b/libs/viz/include/mrpt/viz/registerAllClasses.h new file mode 100644 index 0000000000..0ab54d43eb --- /dev/null +++ b/libs/viz/include/mrpt/viz/registerAllClasses.h @@ -0,0 +1,20 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +namespace mrpt::viz +{ +/** Forces manual RTTI registration of all serializable classes in this + * namespace. Should never be required to be explicitly called by users, except + * if building MRPT as a static library. + * + * \ingroup mrpt_viz_grp + */ +void registerAllClasses_mrpt_viz(); +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/stock_objects.h b/libs/viz/include/mrpt/viz/stock_objects.h new file mode 100644 index 0000000000..cb9e1d1abf --- /dev/null +++ b/libs/viz/include/mrpt/viz/stock_objects.h @@ -0,0 +1,176 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include + +namespace mrpt::viz +{ +/** A collection of pre-built 3D objects for quick insertion in + * opengl::Scene objects. + * \ingroup mrpt_viz_grp + */ +namespace stock_objects +{ +/** Returns a representation of Rhodon. + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + *
+ * + * + *
mrpt::viz::stock_objects::RobotRhodon() \image + * html preview_stock_objects_RobotRhodon.png
+ *
+ */ +CSetOfObjects::Ptr RobotRhodon(); + +/** Returns a representation of RobotGiraff. + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + *
+ * + * + *
mrpt::viz::stock_objects::RobotGiraff() \image + * html preview_stock_objects_RobotGiraff.png
+ *
+ */ +CSetOfObjects::Ptr RobotGiraff(); + +/** Returns a representation of a Pioneer II mobile base. + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + *
+ * + * + *
mrpt::viz::stock_objects::RobotPioneer() \image + * html preview_stock_objects_RobotPioneer.png
+ *
+ */ +CSetOfObjects::Ptr RobotPioneer(); + +/** Returns three arrows representing a X,Y,Z 3D corner. + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + * \sa CornerXYZSimple, CornerXYSimple, CornerXYZEye + *
+ * + * + *
mrpt::viz::stock_objects::CornerXYZ() \image html + * preview_stock_objects_CornerXYZ.png
+ *
+ */ +CSetOfObjects::Ptr CornerXYZ(float scale = 1.0); + +/** Returns three arrows representing a X,Y,Z 3D corner. + * Differently from CornerXYZ the arrowhead of Z axis ends where the object is + * placed. + * This is useful if you want to place this object with the same position and + * orientation of a camera. + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + * \sa CornerXYZSimple, CornerXYSimple + *
+ * + * + *
mrpt::viz::stock_objects::CornerXYZ() \image html + * preview_stock_objects_CornerXYZ.png
+ *
+ */ +CSetOfObjects::Ptr CornerXYZEye(); + +/** Returns three arrows representing a X,Y,Z 3D corner (just thick lines + * instead of complex arrows for faster rendering than CornerXYZ). + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + * \sa CornerXYZ, CornerXYSimple + *
+ * + * + *
mrpt::viz::stock_objects::CornerXYZSimple() + * \image html preview_stock_objects_CornerXYZSimple.png
+ *
+ */ +CSetOfObjects::Ptr CornerXYZSimple(float scale = 1.0, float lineWidth = 1.0); + +/** Returns two arrows representing a X,Y 2D corner (just thick lines, fast to + * render). + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + * \sa CornerXYZSimple, CornerXYZ, CornerXYZEye + *
+ * + * + *
mrpt::viz::stock_objects::CornerXYSimple() \image + * html preview_stock_objects_CornerXYSimple.png
+ *
+ */ +CSetOfObjects::Ptr CornerXYSimple(float scale = 1.0, float lineWidth = 1.0); + +/** Returns a simple 3D model of a PointGrey Bumblebee stereo camera. + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + *
+ * + * + *
mrpt::viz::stock_objects::BumblebeeCamera() + * \image html preview_stock_objects_BumblebeeCamera.png
+ *
+ */ +CSetOfObjects::Ptr BumblebeeCamera(); + +/** Returns a simple 3D model of a Hokuyo URG scanner. + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + *
+ * + * + *
mrpt::viz::stock_objects::Hokuyo_URG() \image + * html preview_stock_objects_Hokuyo_URG.png
+ *
+ */ +CSetOfObjects::Ptr Hokuyo_URG(); + +/** Returns a simple 3D model of a Hokuyo UTM scanner. + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + *
+ * + * + *
mrpt::viz::stock_objects::Hokuyo_UTM() \image + * html preview_stock_objects_Hokuyo_UTM.png
+ *
+ */ +CSetOfObjects::Ptr Hokuyo_UTM(); + +/** Returns a simple 3D model of a househam sprayer. + * The generated object must be inserted in a opengl::Scene or + * opengl::CSetOfObjects. + *
+ * + * + *
mrpt::viz::stock_objects::Househam_Sprayer() + * \image html preview_stock_objects_Househam_Sprayer.png
+ *
+ */ +CSetOfObjects::Ptr Househam_Sprayer(); + +} // namespace stock_objects + +} // namespace mrpt::viz diff --git a/libs/viz/include/mrpt/viz/viz_frwds.h b/libs/viz/include/mrpt/viz/viz_frwds.h new file mode 100644 index 0000000000..48c39c1b06 --- /dev/null +++ b/libs/viz/include/mrpt/viz/viz_frwds.h @@ -0,0 +1,49 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +namespace mrpt::viz +{ +class CArrow; +class CAssimpModel; +class CAxis; +class CBox; +class CCamera; +class CColorBar; +class CCylinder; +class CDisk; +class CEllipsoid3D; +class CEllipsoidInverseDepth2D; +class CEllipsoidInverseDepth3D; +class CEllipsoidRangeBearing2D; +class CFrustum; +class CGridPlaneXY; +class CGridPlaneXZ; +class CMesh; +class CMesh3D; +class CMeshFast; +class COctoMapVoxels; +class Scene; +class Viewport; +class CPointCloud; +class CPointCloudColoured; +class CPolyhedron; +class CSetOfLines; +class CSetOfObjects; +class CSetOfTriangles; +class CSimpleLine; +class CSphere; +class CText; +class CText3D; +class CTexturedPlane; +class CVectorField2D; +class CVectorField3D; + +struct TLightParameters; +} // namespace mrpt::opengl diff --git a/libs/viz/src/CArrow.cpp b/libs/viz/src/CArrow.cpp new file mode 100644 index 0000000000..dc081ac973 --- /dev/null +++ b/libs/viz/src/CArrow.cpp @@ -0,0 +1,109 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include + +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CArrow, CVisualObject, mrpt::viz) + +uint8_t CArrow::serializeGetVersion() const { return 3; } +void CArrow::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_x0 << m_y0 << m_z0; + out << m_x1 << m_y1 << m_z1; + out << m_headRatio << m_smallRadius << m_largeRadius; + out << m_slices; + VisualObjectParams_Triangles::params_serialize(out); // v3 +} + +void CArrow::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + case 3: + { + readFromStreamRender(in); + in >> m_x0 >> m_y0 >> m_z0; + in >> m_x1 >> m_y1 >> m_z1; + in >> m_headRatio >> m_smallRadius >> m_largeRadius; + if (version == 1) + { + float arrow_roll, arrow_pitch, arrow_yaw; + in >> arrow_roll >> arrow_pitch >> arrow_yaw; + } + if (version >= 2) in >> m_slices; + if (version >= 3) VisualObjectParams_Triangles::params_deserialize(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +void CArrow::serializeTo(mrpt::serialization::CSchemeArchiveBase& out) const +{ + SCHEMA_SERIALIZE_DATATYPE_VERSION(1); + out["x0"] = m_x0; + out["y0"] = m_y0; + out["z0"] = m_z0; + out["x1"] = m_x1; + out["y1"] = m_y1; + out["z1"] = m_z1; + out["headRatio"] = m_headRatio; + out["smallRadius"] = m_smallRadius; + out["largeRadius"] = m_largeRadius; + out["slices"] = m_slices; +} + +void CArrow::serializeFrom(mrpt::serialization::CSchemeArchiveBase& in) +{ + uint8_t version; + SCHEMA_DESERIALIZE_DATATYPE_VERSION(); + switch (version) + { + case 1: + { + m_x0 = static_cast(in["x0"]); + m_y0 = static_cast(in["y0"]); + m_z0 = static_cast(in["z0"]); + m_x1 = static_cast(in["x1"]); + m_y1 = static_cast(in["y1"]); + m_z1 = static_cast(in["z1"]); + m_headRatio = static_cast(in["headRatio"]); + m_smallRadius = static_cast(in["smallRadius"]); + m_largeRadius = static_cast(in["largeRadius"]); + m_slices = static_cast(in["slices"]); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + } +} +auto CArrow::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints( + {std::min(m_x0, m_x1), std::min(m_y0, m_y1), std::min(m_z0, m_z1)}, + {std::max(m_x0, m_x1), std::max(m_y0, m_y1), std::max(m_z0, m_z1)}); +} diff --git a/libs/viz/src/CAssimpModel.cpp b/libs/viz/src/CAssimpModel.cpp new file mode 100644 index 0000000000..f5ee8cf920 --- /dev/null +++ b/libs/viz/src/CAssimpModel.cpp @@ -0,0 +1,85 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +// This file contains portions of code from Assimp's example: +// "Sample_SimpleOpenGL.c" + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using mrpt::img::CImage; + +IMPLEMENTS_SERIALIZABLE(CAssimpModel, CVisualObject, mrpt::viz) + +uint8_t CAssimpModel::serializeGetVersion() const { return 0; } +void CAssimpModel::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_modelPath; // v2 + out << m_split_triangles_rendering_bbox; // v3 +} + +void CAssimpModel::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + readFromStreamRender(in); + clear(); + + in >> m_modelPath; + in >> m_split_triangles_rendering_bbox; + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +CAssimpModel::CAssimpModel() {} + +CAssimpModel::~CAssimpModel() { clear(); } + +void CAssimpModel::clear() { m_modelPath.clear(); } + +void CAssimpModel::loadScene(const std::string& filepath, int flags) +{ + clear(); + CVisualObject::notifyChange(); + + m_modelLoadFlags = flags; + m_modelPath = filepath; +} + +auto CAssimpModel::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + THROW_EXCEPTION("Not implemented"); +} + +void CAssimpModel::split_triangles_rendering_bbox(const float bbox_size) +{ + m_split_triangles_rendering_bbox = bbox_size; +} + +bool CAssimpModel::traceRay( + [[maybe_unused]] const mrpt::poses::CPose3D& o, [[maybe_unused]] double& dist) const +{ + // TODO + return false; +} diff --git a/libs/viz/src/CAxis.cpp b/libs/viz/src/CAxis.cpp new file mode 100644 index 0000000000..28372b8feb --- /dev/null +++ b/libs/viz/src/CAxis.cpp @@ -0,0 +1,175 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::system; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CAxis, CVisualObject, mrpt::viz) + +CAxis::CAxis( + float xmin, + float ymin, + float zmin, + float xmax, + float ymax, + float zmax, + float frecuency, + float lineWidth, + bool marks) : + m_xmin(xmin), + m_ymin(ymin), + m_zmin(zmin), + m_xmax(xmax), + m_ymax(ymax), + m_zmax(zmax), + m_frequency(frecuency) +{ + VisualObjectParams_Lines::setLineWidth(lineWidth); + + m_marks.fill(marks); + + // x:180, 0, 90 + m_textRot[0][0] = 180.f; + m_textRot[0][1] = 0.f; + m_textRot[0][2] = 90.f; + // y:90, 0, 90 + m_textRot[1][0] = 90.f; + m_textRot[1][1] = 0.f; + m_textRot[1][2] = 90.f; + // z:180, 0, 90 + m_textRot[2][0] = 180.f; + m_textRot[2][1] = 0.f; + m_textRot[2][2] = 90.f; +} + +void CAxis::setTickMarksLength(float len) +{ + m_markLen = len; + CVisualObject::notifyChange(); +} + +uint8_t CAxis::serializeGetVersion() const { return 3; } +void CAxis::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_xmin << m_ymin << m_zmin; + out << m_xmax << m_ymax << m_zmax; + out << m_frequency; + // v1: + out << m_marks[0] << m_marks[1] << m_marks[2] << m_textScale; + for (auto i : m_textRot) + for (int j = 0; j < 3; j++) out << i[j]; + // v2: + out << m_markLen; + // v3: + this->VisualObjectParams_Lines::params_serialize(out); +} + +void CAxis::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + case 3: + { + readFromStreamRender(in); + in >> m_xmin >> m_ymin >> m_zmin; + in >> m_xmax >> m_ymax >> m_zmax; + in >> m_frequency; + if (version >= 1) + { + in >> m_marks[0] >> m_marks[1] >> m_marks[2] >> m_textScale; + for (auto& i : m_textRot) + for (int j = 0; j < 3; j++) in >> i[j]; + } + else + { + bool v; + in >> v; + m_marks.fill(v); + m_textScale = 0.25f; + } + if (version >= 2) in >> m_markLen; + if (version >= 3) this->VisualObjectParams_Lines::params_deserialize(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +auto CAxis::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints( + {m_xmin, m_ymin, m_zmin}, {m_xmax, m_ymax, m_zmax}); +} + +void CAxis::setFrequency(float f) +{ + ASSERT_(f > 0); + m_frequency = f; + CVisualObject::notifyChange(); +} +float CAxis::getFrequency() const { return m_frequency; } +void CAxis::enableTickMarks(bool v) +{ + m_marks.fill(v); + CVisualObject::notifyChange(); +} +void CAxis::enableTickMarks(bool show_x, bool show_y, bool show_z) +{ + m_marks[0] = show_x; + m_marks[1] = show_y; + m_marks[2] = show_z; + CVisualObject::notifyChange(); +} +void CAxis::setTextScale(float f) +{ + ASSERT_(f > 0); + m_textScale = f; + CVisualObject::notifyChange(); +} +float CAxis::getTextScale() const { return m_textScale; } +void CAxis::setAxisLimits(float xmin, float ymin, float zmin, float xmax, float ymax, float zmax) +{ + m_xmin = xmin; + m_ymin = ymin; + m_zmin = zmin; + m_xmax = xmax; + m_ymax = ymax; + m_zmax = zmax; + CVisualObject::notifyChange(); +} +void CAxis::setTextLabelOrientation(int axis, float yaw_deg, float pitch_deg, float roll_deg) +{ + ASSERT_(axis >= 0 && axis < 3); + m_textRot[axis][0] = yaw_deg; + m_textRot[axis][1] = pitch_deg; + m_textRot[axis][2] = roll_deg; +} +void CAxis::getTextLabelOrientation( + int axis, float& yaw_deg, float& pitch_deg, float& roll_deg) const +{ + ASSERT_(axis >= 0 && axis < 3); + yaw_deg = m_textRot[axis][0]; + pitch_deg = m_textRot[axis][1]; + roll_deg = m_textRot[axis][2]; +} diff --git a/libs/viz/src/CBox.cpp b/libs/viz/src/CBox.cpp new file mode 100644 index 0000000000..1a94ed46a2 --- /dev/null +++ b/libs/viz/src/CBox.cpp @@ -0,0 +1,101 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CBox, CVisualObject, mrpt::viz) + +CBox::CBox( + const mrpt::math::TPoint3D& corner1, + const mrpt::math::TPoint3D& corner2, + bool is_wireframe, + float lineWidth) : + m_wireframe(is_wireframe) +{ + VisualObjectParams_Lines::setLineWidth(lineWidth); + setBoxCorners(corner1, corner2); +} + +uint8_t CBox::serializeGetVersion() const { return 3; } +void CBox::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + // version 0 + out << m_corner_min.x << m_corner_min.y << m_corner_min.z << m_corner_max.x << m_corner_max.y + << m_corner_max.z << m_wireframe; + // Version 1: + out << m_draw_border << m_solidborder_color; + VisualObjectParams_Triangles::params_serialize(out); // v2 + VisualObjectParams_Lines::params_serialize(out); // v3 +} + +void CBox::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + case 3: + { + readFromStreamRender(in); + in >> m_corner_min.x >> m_corner_min.y >> m_corner_min.z >> m_corner_max.x >> + m_corner_max.y >> m_corner_max.z >> m_wireframe; + // Version 1: + if (version >= 1) + in >> m_draw_border >> m_solidborder_color; + else + { + m_draw_border = false; + } + if (version >= 2) VisualObjectParams_Triangles::params_deserialize(in); + if (version >= 3) VisualObjectParams_Lines::params_deserialize(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +void CBox::setBoxCorners(const mrpt::math::TPoint3D& corner1, const mrpt::math::TPoint3D& corner2) +{ + CVisualObject::notifyChange(); + + // Order the coordinates so we always have the min/max in their right + // position: + m_corner_min.x = std::min(corner1.x, corner2.x); + m_corner_min.y = std::min(corner1.y, corner2.y); + m_corner_min.z = std::min(corner1.z, corner2.z); + + m_corner_max.x = std::max(corner1.x, corner2.x); + m_corner_max.y = std::max(corner1.y, corner2.y); + m_corner_max.z = std::max(corner1.z, corner2.z); +} + +bool CBox::traceRay( + [[maybe_unused]] const mrpt::poses::CPose3D& o, [[maybe_unused]] double& dist) const +{ + THROW_EXCEPTION("TO DO"); +} + +auto CBox::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints( + m_corner_min.cast(), m_corner_max.cast()); +} diff --git a/libs/viz/src/CCamera.cpp b/libs/viz/src/CCamera.cpp new file mode 100644 index 0000000000..3cd9528115 --- /dev/null +++ b/libs/viz/src/CCamera.cpp @@ -0,0 +1,93 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CCamera, CVisualObject, mrpt::viz) + +uint8_t CCamera::serializeGetVersion() const { return 4; } +void CCamera::serializeTo(mrpt::serialization::CArchive& out) const +{ + // Save data: + out << m_pointingX << m_pointingY << m_pointingZ << m_eyeDistance << m_azimuthDeg + << m_elevationDeg << m_projectiveModel << m_projectiveFOVdeg; + out << m_pinholeModel; // v2 + out << m_useNoProjection; // v3 + out << m_eyeRollDeg; // v4 +} + +void CCamera::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 1: + case 2: + case 3: + case 4: + { + // Load data: + in >> m_pointingX >> m_pointingY >> m_pointingZ >> m_eyeDistance >> m_azimuthDeg >> + m_elevationDeg >> m_projectiveModel >> m_projectiveFOVdeg; + if (version >= 2) + in >> m_pinholeModel; + else + m_pinholeModel.reset(); + + if (version >= 3) + in >> m_useNoProjection; + else + m_useNoProjection = false; + + if (version >= 4) + in >> m_eyeRollDeg; + else + m_eyeRollDeg = 0; + } + break; + case 0: + { + in >> m_pointingX >> m_pointingY >> m_pointingZ >> m_eyeDistance >> m_azimuthDeg >> + m_elevationDeg; + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +/** In this class, returns a fixed box (max,max,max), (-max,-max,-max). */ +auto CCamera::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf { return {}; } + +void CCamera::toYAMLMap(mrpt::containers::yaml& p) const +{ + CVisualObject::toYAMLMap(p); + + MCP_SAVE(p, m_pointingX); + MCP_SAVE(p, m_pointingY); + MCP_SAVE(p, m_pointingZ); + MCP_SAVE(p, m_eyeDistance); + MCP_SAVE(p, m_azimuthDeg); + MCP_SAVE(p, m_elevationDeg); + MCP_SAVE(p, m_projectiveModel); + MCP_SAVE(p, m_projectiveFOVdeg); + MCP_SAVE(p, m_useNoProjection); + MCP_SAVE(p, m_eyeRollDeg); + + if (m_pinholeModel) p["pinholeModel"] = m_pinholeModel->asYAML(); +} diff --git a/libs/viz/src/CColorBar.cpp b/libs/viz/src/CColorBar.cpp new file mode 100644 index 0000000000..2641451eae --- /dev/null +++ b/libs/viz/src/CColorBar.cpp @@ -0,0 +1,105 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CColorBar, CVisualObject, mrpt::viz) + +CColorBar::CColorBar( + /** The colormap to represent. */ + const mrpt::img::TColormap colormap, + /** size of the color bar */ + double width, + double height, + /** limits for [0,1] colormap indices */ + float min_col, + float max_col, + /** limits for values associated to extreme colors */ + float min_value, + float max_value, + /** sprintf-like format string for values */ + const std::string& label_format, + /** Label text font size */ + float label_font_size) : + m_colormap(colormap), + m_width(width), + m_height(height), + m_label_format(label_format), + m_min_col(min_col), + m_max_col(max_col), + m_min_value(min_value), + m_max_value(max_value), + m_label_font_size(label_font_size) +{ +} + +void CColorBar::setColormap(const mrpt::img::TColormap colormap) +{ + m_colormap = colormap; + CVisualObject::notifyChange(); +} + +void CColorBar::setColorAndValueLimits( + float col_min, float col_max, float value_min, float value_max) +{ + m_min_col = col_min; + m_max_col = col_max; + m_min_value = value_min; + m_max_value = value_max; + CVisualObject::notifyChange(); +} + +uint8_t CColorBar::serializeGetVersion() const { return 2; } +void CColorBar::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + // version 0 + out << uint32_t(m_colormap) << m_min_col << m_max_col << m_min_value << m_max_value + << m_label_format << m_label_font_size; + VisualObjectParams_Triangles::params_serialize(out); // v2 +} +void CColorBar::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + readFromStreamRender(in); + + in.ReadAsAndCastTo(m_colormap); + in >> m_min_col >> m_max_col >> m_min_value >> m_max_value >> m_label_format >> + m_label_font_size; + if (version == 0) + { + bool old_disable_depth_test; + in >> old_disable_depth_test; + } + if (version >= 2) VisualObjectParams_Triangles::params_deserialize(in); + + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +mrpt::math::TBoundingBoxf CColorBar::internalBoundingBoxLocal() const +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints( + {.0f, .0f, .0f}, {d2f(m_width), d2f(m_height), .0f}); +} diff --git a/libs/viz/src/CCylinder.cpp b/libs/viz/src/CCylinder.cpp new file mode 100644 index 0000000000..07cf11bb47 --- /dev/null +++ b/libs/viz/src/CCylinder.cpp @@ -0,0 +1,210 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CCylinder, CVisualObject, mrpt::viz) + +void CCylinder::serializeTo(mrpt::serialization::CSchemeArchiveBase& out) const +{ + SCHEMA_SERIALIZE_DATATYPE_VERSION(1); + out["baseRadius"] = m_baseRadius; + out["topRadius"] = m_topRadius; + out["height"] = m_height; + out["slices"] = m_slices; + out["hasBottomBase"] = m_hasBottomBase; + out["hasTopBase"] = m_hasTopBase; +} +void CCylinder::serializeFrom(mrpt::serialization::CSchemeArchiveBase& in) +{ + uint8_t version; + SCHEMA_DESERIALIZE_DATATYPE_VERSION(); + switch (version) + { + case 1: + { + m_baseRadius = static_cast(in["baseRadius"]); + m_topRadius = static_cast(in["topRadius"]); + m_height = static_cast(in["height"]); + m_slices = static_cast(in["slices"]); + m_hasBottomBase = static_cast(in["hasBottomBase"]); + m_hasTopBase = static_cast(in["hasTopBase"]); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + } +} +uint8_t CCylinder::serializeGetVersion() const { return 2; } +void CCylinder::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + // version 0 + out << m_baseRadius << m_topRadius << m_height << m_slices << m_hasBottomBase << m_hasTopBase; + VisualObjectParams_Triangles::params_serialize(out); // v2 +} +void CCylinder::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + readFromStreamRender(in); + in >> m_baseRadius >> m_topRadius >> m_height >> m_slices; + + if (version < 1) + { + float old_mStacks; + in >> old_mStacks; + } + + in >> m_hasBottomBase >> m_hasTopBase; + + if (version >= 2) VisualObjectParams_Triangles::params_deserialize(in); + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +namespace +{ +bool solveEqn(double a, double b, double c, double& t) +{ // Actually, the b from the quadratic equation is + // the DOUBLE of this. But + // this way, operations are simpler. + if (a < 0) + { + a = -a; + b = -b; + c = -c; + } + if (a >= mrpt::math::getEpsilon()) + { + double delta = square(b) - a * c; + if (delta == 0) + return (t = -b / a) >= 0; + else if (delta >= 0) + { + delta = sqrt(delta); + if (-b - delta > 0) + { + t = (-b - delta) / a; + return true; + } + else if (-b + delta > 0) + { + t = (-b + delta) / a; + return true; + } // else return false; Both solutions are negative + } // else return false; Both solutions are complex + } + else if (std::abs(b) >= mrpt::math::getEpsilon()) + { + t = -c / (b + b); + return t >= 0; + } // else return false; This actually isn't an equation + return false; +} +} // namespace + +bool CCylinder::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ + TLine3D lin; + mrpt::math::createFromPoseX((o - getCPose()).asTPose(), lin); + lin.unitarize(); // By adding this line, distance from any point of the + + const float zz = d2f(lin.pBase.z); + + // line to its base is exactly equal to the "t". + if (std::abs(lin.director[2]) < getEpsilon()) + { + if (!reachesHeight(zz)) return false; + float r; + return getRadius(zz, r) ? solveEqn( + square(lin.director[0]) + square(lin.director[1]), + lin.director[0] * lin.pBase.x + lin.director[1] * lin.pBase.y, + square(lin.pBase.x) + square(lin.pBase.y) - square(r), dist) + : false; + } + bool fnd = false; + double nDist, tZ0; + if (m_hasBottomBase && (tZ0 = -lin.pBase.z / lin.director[2]) > 0) + { + nDist = sqrt( + square(lin.pBase.x + tZ0 * lin.director[0]) + square(lin.pBase.y + tZ0 * lin.director[1])); + if (nDist <= m_baseRadius) + { + fnd = true; + dist = tZ0; + } + } + if (m_hasTopBase) + { + tZ0 = (m_height - lin.pBase.z) / lin.director[2]; + if (tZ0 > 0 && (!fnd || tZ0 < dist)) + { + nDist = sqrt( + square(lin.pBase.x + tZ0 * lin.director[0]) + + square(lin.pBase.y + tZ0 * lin.director[1])); + if (nDist <= m_topRadius) + { + fnd = true; + dist = tZ0; + } + } + } + if (m_baseRadius == m_topRadius) + { + if (solveEqn( + square(lin.director[0]) + square(lin.director[1]), + lin.director[0] * lin.pBase.x + lin.director[1] * lin.pBase.y, + square(lin.pBase.x) + square(lin.pBase.y) - square(m_baseRadius), nDist)) + if ((!fnd || nDist < dist) && reachesHeight(lin.pBase.z + nDist * lin.director[2])) + { + dist = nDist; + fnd = true; + } + } + else + { + double slope = (m_topRadius - m_baseRadius) / m_height; + if (solveEqn( + square(lin.director[0]) + square(lin.director[1]) - square(lin.director[2] * slope), + lin.pBase.x * lin.director[0] + lin.pBase.y * lin.director[1] - + (m_baseRadius + slope * lin.pBase.z) * slope * lin.director[2], + square(lin.pBase.x) + square(lin.pBase.y) - square(m_baseRadius + slope * lin.pBase.z), + nDist)) + if ((!fnd || nDist < dist) && reachesHeight(lin.pBase.z + nDist * lin.director[2])) + { + dist = nDist; + fnd = true; + } + } + return fnd; +} + +auto CCylinder::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + const float R = std::max(m_baseRadius, m_topRadius); + return mrpt::math::TBoundingBoxf::FromUnsortedPoints({-R, -R, 0}, {R, R, m_height}); +} diff --git a/libs/viz/src/CDisk.cpp b/libs/viz/src/CDisk.cpp new file mode 100644 index 0000000000..a1bf84a984 --- /dev/null +++ b/libs/viz/src/CDisk.cpp @@ -0,0 +1,93 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace std; +using mrpt::poses::CPose3D; + +IMPLEMENTS_SERIALIZABLE(CDisk, CVisualObject, mrpt::viz) + +uint8_t CDisk::serializeGetVersion() const { return 2; } +void CDisk::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_radiusIn << m_radiusOut; + out << m_nSlices; + VisualObjectParams_Triangles::params_serialize(out); // v2 +} + +void CDisk::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + { + readFromStreamRender(in); + in >> m_radiusIn >> m_radiusOut; + in >> m_nSlices; + if (version < 1) + { + float dummy_loops; + in >> dummy_loops; + } + + if (version >= 2) VisualObjectParams_Triangles::params_deserialize(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +bool CDisk::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ + // The disk is contained initially in a plane which contains (0,0,0), + // (1,0,0) and (0,1,0) + // These points are converted into: + //(x,y,z) + //( cos(w)*cos(p)+x, sin(w)*cos(p)*y, -sin(p)+z ) + //( -sin(w)*cos(r)+cos(w)*sin(p)*sin(r)+x, + // cos(w)*cos(r)+sin(w)*sin(p)*sin(r)+y, cos(p)*sin(r)*z ) + CPose3D transf = getCPose() - o; + double x = transf.x(), y = transf.y(), z = transf.z(), w = transf.yaw(), p = transf.pitch(), + r = transf.roll(); + double coef = sin(w) * sin(r) + cos(w) * sin(p) * cos(r); + // coef is the first component of the normal to the transformed Z plane. So, + // the scalar product between + // this normal and (1,0,0) (which happens to be the beam's vector) equals + // coef. And if it's 0, then both + // are orthogonal, that is, the beam is parallel to the plane. + if (coef == 0) return false; + // The following expression yields the collision point between the plane and + // the beam (the y and z + // coordinates are zero). + dist = x + (y * (sin(p) * sin(w) * cos(r) - cos(w) * sin(r)) + z * cos(p) * cos(r)) / coef; + if (dist < 0) return false; + // Euclidean distance is invariant to rotations... + double d2 = (x - dist) * (x - dist) + y * y + z * z; + return d2 >= (m_radiusIn * m_radiusIn) && d2 <= (m_radiusOut * m_radiusOut); + + // IMPORTANT NOTICE: using geometric intersection between Z plane and + // CPose's line intersection is SLOWER than the used method. +} + +mrpt::math::TBoundingBoxf CDisk::internalBoundingBoxLocal() const +{ + const float R = std::max(m_radiusIn, m_radiusOut); + return mrpt::math::TBoundingBoxf::FromUnsortedPoints({-R, -R, 0}, {R, R, .0}); +} diff --git a/libs/viz/src/CEllipsoid2D.cpp b/libs/viz/src/CEllipsoid2D.cpp new file mode 100644 index 0000000000..094c7f6c32 --- /dev/null +++ b/libs/viz/src/CEllipsoid2D.cpp @@ -0,0 +1,119 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CEllipsoid2D, CVisualObject, mrpt::viz) + +void CEllipsoid2D::transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const +{ + // Euclidean space: + out_pts = in_pts; +} + +uint8_t CEllipsoid2D::serializeGetVersion() const { return 1; } +void CEllipsoid2D::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_numSegments; + VisualObjectParams_Lines::params_serialize(out); +} + +void CEllipsoid2D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + { + readFromStreamRender(in); + in >> m_cov; + in >> m_drawSolid3D >> m_quantiles; + m_numSegments = in.ReadAs(); + + if (version >= 1) + { + VisualObjectParams_Lines::params_deserialize(in); + } + else + { + VisualObjectParams_Lines::setLineWidth(in.ReadAs()); + } + + // Update cov. matrix cache: + setCovMatrix(m_cov); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +#if 0 +static bool quickSolveEqn(double a, double b_2, double c, double& t) +{ + double delta = square(b_2) - a * c; + if (delta == 0) + return (t = -b_2 / a) >= 0; + else if (delta > 0) + { + delta = sqrt(delta); + if ((t = (-b_2 - delta) / a) >= 0) + return true; + else + return (t = (-b_2 + delta) / a) >= 0; + } + else + return false; +} +#endif + +bool CEllipsoid2D::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ +#if 0 // Update, someday... + if (m_cov.rows() != 3) return false; + TLine3D lin, lin2; + createFromPoseX((o - getCPose()).asTPose(), lin); + lin.unitarize(); // By adding this line, distance from any point of the + // line to its base is exactly equal to the "t". + for (size_t i = 0; i < 3; i++) + { + lin2.pBase[i] = 0; + lin2.director[i] = 0; + for (size_t j = 0; j < 3; j++) + { + double vji = m_eigVec(j, i); + lin2.pBase[i] += vji * lin.pBase[j]; + lin2.director[i] += vji * lin.director[j]; + } + } + double a = 0, b_2 = 0, c = -square(m_quantiles); + for (size_t i = 0; i < 3; i++) + { + double ev = m_eigVal(i, i); + a += square(lin2.director[i] / ev); + b_2 += lin2.director[i] * lin2.pBase[i] / square(ev); + c += square(lin2.pBase[i] / ev); + } + return quickSolveEqn(a, b_2, c, dist); +#endif + return false; +} diff --git a/libs/viz/src/CEllipsoid3D.cpp b/libs/viz/src/CEllipsoid3D.cpp new file mode 100644 index 0000000000..c5ba2ab7d6 --- /dev/null +++ b/libs/viz/src/CEllipsoid3D.cpp @@ -0,0 +1,120 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CEllipsoid3D, CVisualObject, mrpt::viz) + +void CEllipsoid3D::transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const +{ + // Euclidean space: + out_pts = in_pts; +} + +uint8_t CEllipsoid3D::serializeGetVersion() const { return 1; } +void CEllipsoid3D::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_numSegments; + VisualObjectParams_Lines::params_serialize(out); // v1 +} + +void CEllipsoid3D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + { + readFromStreamRender(in); + in >> m_cov; + in >> m_drawSolid3D >> m_quantiles; + m_numSegments = in.ReadAs(); + if (version >= 1) + { + VisualObjectParams_Lines::params_deserialize(in); + } + else + { + VisualObjectParams_Lines::setLineWidth(in.ReadAs()); + } + + // Update cov. matrix cache: + setCovMatrix(m_cov); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +#if 0 +static bool quickSolveEqn(double a, double b_2, double c, double& t) +{ + double delta = square(b_2) - a * c; + if (delta == 0) + return (t = -b_2 / a) >= 0; + else if (delta > 0) + { + delta = sqrt(delta); + if ((t = (-b_2 - delta) / a) >= 0) + return true; + else + return (t = (-b_2 + delta) / a) >= 0; + } + else + return false; +} +#endif + +bool CEllipsoid3D::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ +#if 0 // Update, someday... + if (m_cov.rows() != 3) return false; + TLine3D lin, lin2; + createFromPoseX((o - getCPose()).asTPose(), lin); + lin.unitarize(); // By adding this line, distance from any point of the + // line to its base is exactly equal to the "t". + for (size_t i = 0; i < 3; i++) + { + lin2.pBase[i] = 0; + lin2.director[i] = 0; + for (size_t j = 0; j < 3; j++) + { + double vji = m_eigVec(j, i); + lin2.pBase[i] += vji * lin.pBase[j]; + lin2.director[i] += vji * lin.director[j]; + } + } + double a = 0, b_2 = 0, c = -square(m_quantiles); + for (size_t i = 0; i < 3; i++) + { + double ev = m_eigVal(i, i); + a += square(lin2.director[i] / ev); + b_2 += lin2.director[i] * lin2.pBase[i] / square(ev); + c += square(lin2.pBase[i] / ev); + } + return quickSolveEqn(a, b_2, c, dist); +#endif + return false; +} diff --git a/libs/viz/src/CEllipsoidInverseDepth2D.cpp b/libs/viz/src/CEllipsoidInverseDepth2D.cpp new file mode 100644 index 0000000000..b9d2481f04 --- /dev/null +++ b/libs/viz/src/CEllipsoidInverseDepth2D.cpp @@ -0,0 +1,74 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include // for << >> ops of matrices +#include +#include + +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CEllipsoidInverseDepth2D, CVisualObject, mrpt::viz) + +/*--------------------------------------------------------------- + transformFromParameterSpace + ---------------------------------------------------------------*/ +void CEllipsoidInverseDepth2D::transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const +{ + MRPT_START + + // (inv_range,yaw) --> (x,y) + const size_t N = in_pts.size(); + out_pts.resize(N); + for (size_t i = 0; i < N; i++) + { + const double inv_range = in_pts[i][0]; + const double yaw = in_pts[i][1]; + const double range = + inv_range < 0 ? m_underflowMaxRange : (inv_range != 0 ? 1. / inv_range : 0); + out_pts[i][0] = d2f(range * cos(yaw)); + out_pts[i][1] = d2f(range * sin(yaw)); + } + + MRPT_END +} + +uint8_t CEllipsoidInverseDepth2D::serializeGetVersion() const { return 0; } +void CEllipsoidInverseDepth2D::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + BASE::thisclass_writeToStream(out); + + out << m_underflowMaxRange; +} +void CEllipsoidInverseDepth2D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + readFromStreamRender(in); + BASE::thisclass_readFromStream(in); + + in >> m_underflowMaxRange; + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} diff --git a/libs/viz/src/CEllipsoidInverseDepth3D.cpp b/libs/viz/src/CEllipsoidInverseDepth3D.cpp new file mode 100644 index 0000000000..096e53d26d --- /dev/null +++ b/libs/viz/src/CEllipsoidInverseDepth3D.cpp @@ -0,0 +1,76 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CEllipsoidInverseDepth3D, CVisualObject, mrpt::viz) + +/*--------------------------------------------------------------- + transformFromParameterSpace + ---------------------------------------------------------------*/ +void CEllipsoidInverseDepth3D::transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const +{ + MRPT_START + + // (inv_range,yaw,pitch) --> (x,y,z) + const size_t N = in_pts.size(); + out_pts.resize(N); + for (size_t i = 0; i < N; i++) + { + const float inv_range = in_pts[i][0]; + const float yaw = in_pts[i][1]; + const float pitch = in_pts[i][2]; + + const float range = + inv_range < 0 ? m_underflowMaxRange : (inv_range != 0 ? 1.f / inv_range : 0); + + out_pts[i][0] = range * cosf(yaw) * cosf(pitch); + out_pts[i][1] = range * sinf(yaw) * cosf(pitch); + out_pts[i][2] = -range * sinf(pitch); + } + + MRPT_END +} + +uint8_t CEllipsoidInverseDepth3D::serializeGetVersion() const { return 0; } +void CEllipsoidInverseDepth3D::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + BASE::thisclass_writeToStream(out); + + out << m_underflowMaxRange; +} + +void CEllipsoidInverseDepth3D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + readFromStreamRender(in); + BASE::thisclass_readFromStream(in); + + in >> m_underflowMaxRange; + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} diff --git a/libs/viz/src/CEllipsoidRangeBearing2D.cpp b/libs/viz/src/CEllipsoidRangeBearing2D.cpp new file mode 100644 index 0000000000..2f4f8e5e82 --- /dev/null +++ b/libs/viz/src/CEllipsoidRangeBearing2D.cpp @@ -0,0 +1,66 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CEllipsoidRangeBearing2D, CVisualObject, mrpt::viz) + +/*--------------------------------------------------------------- + transformFromParameterSpace + ---------------------------------------------------------------*/ +void CEllipsoidRangeBearing2D::transformFromParameterSpace( + const std::vector& in_pts, + std::vector& out_pts) const +{ + MRPT_START + + // (range,bearing) --> (x,y) + const size_t N = in_pts.size(); + out_pts.resize(N); + for (size_t i = 0; i < N; i++) + { + const float range = in_pts[i][0]; + const float bearing = in_pts[i][1]; + out_pts[i][0] = range * cosf(bearing); + out_pts[i][1] = range * sinf(bearing); + } + + MRPT_END +} + +uint8_t CEllipsoidRangeBearing2D::serializeGetVersion() const { return 0; } +void CEllipsoidRangeBearing2D::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + BASE::thisclass_writeToStream(out); +} + +void CEllipsoidRangeBearing2D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + readFromStreamRender(in); + BASE::thisclass_readFromStream(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} diff --git a/libs/viz/src/CFrustum.cpp b/libs/viz/src/CFrustum.cpp new file mode 100644 index 0000000000..74261fb789 --- /dev/null +++ b/libs/viz/src/CFrustum.cpp @@ -0,0 +1,197 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CFrustum, CVisualObject, mrpt::viz) + +std::array CFrustum::computeFrustumCorners() const +{ + std::array pts; + for (size_t j = 0; j < 2; j++) + { + const float r = j == 0 ? m_min_distance : m_max_distance; + for (size_t i = 0; i < 4; i++) pts[4 * j + i].x = r; + pts[4 * j + 0].y = -r * tan(m_fov_horz_left); + pts[4 * j + 1].y = -r * tan(m_fov_horz_left); + pts[4 * j + 2].y = r * tan(m_fov_horz_right); + pts[4 * j + 3].y = r * tan(m_fov_horz_right); + pts[4 * j + 0].z = -r * tan(m_fov_vert_down); + pts[4 * j + 1].z = r * tan(m_fov_vert_up); + pts[4 * j + 2].z = -r * tan(m_fov_vert_down); + pts[4 * j + 3].z = r * tan(m_fov_vert_up); + } + return pts; +} + +// Ctors +CFrustum::CFrustum() : + m_fov_horz_left(mrpt::DEG2RAD(45.0f)), + m_fov_horz_right(mrpt::DEG2RAD(45.0f)), + m_fov_vert_down(mrpt::DEG2RAD(30.0f)), + m_fov_vert_up(mrpt::DEG2RAD(30.0f)), + m_planes_color(0xE0, 0x00, 0x00, 0x50) // RGBA +{ + keep_min(m_fov_horz_left, DEG2RAD(89.9f)); + keep_max(m_fov_horz_left, 0); + keep_min(m_fov_horz_right, DEG2RAD(89.9f)); + keep_max(m_fov_horz_right, 0); + keep_min(m_fov_vert_down, DEG2RAD(89.9f)); + keep_max(m_fov_vert_down, 0); + keep_min(m_fov_vert_up, DEG2RAD(89.9f)); + keep_max(m_fov_vert_up, 0); +} + +CFrustum::CFrustum( + float near_distance, + float far_distance, + float horz_FOV_degrees, + float vert_FOV_degrees, + float lineWidth, + bool draw_lines, + bool draw_planes) : + m_min_distance(near_distance), + m_max_distance(far_distance), + m_fov_horz_left(mrpt::DEG2RAD(.5f * horz_FOV_degrees)), + m_fov_horz_right(mrpt::DEG2RAD(.5f * horz_FOV_degrees)), + m_fov_vert_down(mrpt::DEG2RAD(.5f * vert_FOV_degrees)), + m_fov_vert_up(mrpt::DEG2RAD(.5f * vert_FOV_degrees)), + m_draw_lines(draw_lines), + m_draw_planes(draw_planes), + m_planes_color(0xE0, 0x00, 0x00, 0x50) // RGBA +{ + this->setLineWidth(lineWidth); +} + +uint8_t CFrustum::serializeGetVersion() const { return 2; } +void CFrustum::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + // version 0 + out << m_min_distance << m_max_distance << m_fov_horz_left << m_fov_horz_right << m_fov_vert_down + << m_fov_vert_up << m_draw_lines << m_draw_planes; + VisualObjectParams_Lines::params_serialize(out); // was: m_lineWidth; // v2 + out << m_planes_color.R << m_planes_color.G << m_planes_color.B << m_planes_color.A; + VisualObjectParams_Triangles::params_serialize(out); // v1 +} + +void CFrustum::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + readFromStreamRender(in); + in >> m_min_distance >> m_max_distance >> m_fov_horz_left >> m_fov_horz_right >> + m_fov_vert_down >> m_fov_vert_up >> m_draw_lines >> m_draw_planes; + if (version < 2) + { + setLineWidth(in.ReadAs()); + } + else + { + VisualObjectParams_Lines::params_deserialize(in); + } + + in >> m_planes_color.R >> m_planes_color.G >> m_planes_color.B >> m_planes_color.A; + + if (version >= 1) VisualObjectParams_Triangles::params_deserialize(in); + + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +bool CFrustum::traceRay( + [[maybe_unused]] const mrpt::poses::CPose3D& o, [[maybe_unused]] double& dist) const +{ + THROW_EXCEPTION("TO DO"); +} + +// setters: +void CFrustum::setNearFarPlanes(const float near_distance, const float far_distance) +{ + m_min_distance = near_distance; + m_max_distance = far_distance; + CVisualObject::notifyChange(); +} +void CFrustum::setHorzFOV(const float fov_horz_degrees) +{ + m_fov_horz_right = m_fov_horz_left = 0.5f * mrpt::DEG2RAD(fov_horz_degrees); + keep_min(m_fov_horz_left, DEG2RAD(89.9f)); + keep_max(m_fov_horz_left, 0); + keep_min(m_fov_horz_right, DEG2RAD(89.9f)); + keep_max(m_fov_horz_right, 0); + CVisualObject::notifyChange(); +} +void CFrustum::setVertFOV(const float fov_vert_degrees) +{ + m_fov_vert_down = m_fov_vert_up = 0.5f * mrpt::DEG2RAD(fov_vert_degrees); + keep_min(m_fov_vert_down, DEG2RAD(89.9f)); + keep_max(m_fov_vert_down, 0); + keep_min(m_fov_vert_up, DEG2RAD(89.9f)); + keep_max(m_fov_vert_up, 0); + CVisualObject::notifyChange(); +} +void CFrustum::setHorzFOVAsymmetric( + const float fov_horz_left_degrees, const float fov_horz_right_degrees) +{ + m_fov_horz_left = mrpt::DEG2RAD(fov_horz_left_degrees); + m_fov_horz_right = mrpt::DEG2RAD(fov_horz_right_degrees); + keep_min(m_fov_horz_left, DEG2RAD(89.9f)); + keep_max(m_fov_horz_left, 0); + keep_min(m_fov_horz_right, DEG2RAD(89.9f)); + keep_max(m_fov_horz_right, 0); + CVisualObject::notifyChange(); +} +void CFrustum::setVertFOVAsymmetric( + const float fov_vert_down_degrees, const float fov_vert_up_degrees) +{ + m_fov_vert_down = mrpt::DEG2RAD(fov_vert_down_degrees); + m_fov_vert_up = mrpt::DEG2RAD(fov_vert_up_degrees); + keep_min(m_fov_vert_down, DEG2RAD(89.9f)); + keep_max(m_fov_vert_down, 0); + keep_min(m_fov_vert_up, DEG2RAD(89.9f)); + keep_max(m_fov_vert_up, 0); + CVisualObject::notifyChange(); +} + +auto CFrustum::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + std::array pts = computeFrustumCorners(); + + auto bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); + + for (const auto& pt : pts) bb.updateWithPoint(pt); + + return bb; +} + +CFrustum::CFrustum(const mrpt::img::TCamera& i, double focalScale) : + CFrustum( + i.fx() * focalScale * 0.1f, + i.fx() * focalScale, + 2 * mrpt::RAD2DEG(std::atan2(i.ncols, 2 * i.fx())), + 2 * mrpt::RAD2DEG(std::atan2(i.nrows, 2 * i.fy())), + 1.0f, + true, + false) +{ +} diff --git a/libs/viz/src/CGeneralizedEllipsoidTemplate.cpp b/libs/viz/src/CGeneralizedEllipsoidTemplate.cpp new file mode 100644 index 0000000000..d4c113c83b --- /dev/null +++ b/libs/viz/src/CGeneralizedEllipsoidTemplate.cpp @@ -0,0 +1,111 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +template <> +void CGeneralizedEllipsoidTemplate<2>::generatePoints( + const CGeneralizedEllipsoidTemplate<2>::cov_matrix_t& U, + std::vector::array_parameter_t>& pts) const +{ + pts.clear(); + pts.reserve(m_numSegments); + const double Aa = 2 * M_PI / m_numSegments; + for (double ang = 0; ang < 2 * M_PI; ang += Aa) + { + const double ccos = cos(ang); + const double ssin = sin(ang); + + pts.resize(pts.size() + 1); + + auto& pt = pts.back(); + + pt[0] = d2f(m_mean[0] + ccos * U(0, 0) + ssin * U(0, 1)); + pt[1] = d2f(m_mean[1] + ccos * U(1, 0) + ssin * U(1, 1)); + } +} + +void aux_add3DpointWithEigenVectors( + const double x, + const double y, + const double z, + std::vector>& pts, + const mrpt::math::CMatrixFixed& M, + const mrpt::math::CMatrixFixed& mean) +{ + pts.resize(pts.size() + 1); + mrpt::math::CMatrixFixed& pt = pts.back(); + pt[0] = d2f(mean[0] + x * M(0, 0) + y * M(0, 1) + z * M(0, 2)); + pt[1] = d2f(mean[1] + x * M(1, 0) + y * M(1, 1) + z * M(1, 2)); + pt[2] = d2f(mean[2] + x * M(2, 0) + y * M(2, 1) + z * M(2, 2)); +} + +template <> +void CGeneralizedEllipsoidTemplate<3>::generatePoints( + const CGeneralizedEllipsoidTemplate<3>::cov_matrix_t& U, + std::vector::array_parameter_t>& pts) const +{ + MRPT_START + const auto slices = m_numSegments, stacks = m_numSegments; + ASSERT_GE_(slices, 3); + ASSERT_GE_(stacks, 3); + // sin/cos cache -------- + // Slices: [0,pi] + std::vector slice_cos(slices), slice_sin(slices); + for (uint32_t i = 0; i < slices; i++) + { + double angle = M_PI * i / double(slices - 1); + slice_sin[i] = sin(angle); + slice_cos[i] = cos(angle); + } + // Stacks: [0,2*pi] + std::vector stack_sin(stacks), stack_cos(stacks); + for (uint32_t i = 0; i < stacks; i++) + { + double angle = 2 * M_PI * i / double(stacks); + stack_sin[i] = sin(angle); + stack_cos[i] = cos(angle); + } + + // Points in the ellipsoid: + // * "#slices" slices, with "#stacks" points each, but for the two ends + // * 1 point at each end slice + // #total points = stacks*(slices-2) + 2 + pts.clear(); + pts.reserve((slices - 2) * stacks + 2); + + for (uint32_t i = 0; i < slices; i++) + { + if (i == 0) + aux_add3DpointWithEigenVectors(1, 0, 0, pts, U, m_mean); + else if (i == (slices - 1)) + aux_add3DpointWithEigenVectors(-1, 0, 0, pts, U, m_mean); + else + { + const double x = slice_cos[i]; + const double R = slice_sin[i]; + + for (uint32_t j = 0; j < stacks; j++) + { + const double y = R * stack_cos[j]; + const double z = R * stack_sin[j]; + aux_add3DpointWithEigenVectors(x, y, z, pts, U, m_mean); + } + } + } + + MRPT_END +} diff --git a/libs/viz/src/CGridPlaneXY.cpp b/libs/viz/src/CGridPlaneXY.cpp new file mode 100644 index 0000000000..de10900eda --- /dev/null +++ b/libs/viz/src/CGridPlaneXY.cpp @@ -0,0 +1,79 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CGridPlaneXY, CVisualObject, mrpt::viz) + +/** Constructor */ +CGridPlaneXY::CGridPlaneXY( + float xMin, + float xMax, + float yMin, + float yMax, + float z, + float frequency, + float lineWidth, + bool antiAliasing) : + m_xMin(xMin), m_xMax(xMax), m_yMin(yMin), m_yMax(yMax), m_plane_z(z), m_frequency(frequency) +{ + setLineWidth(lineWidth); + enableAntiAliasing(antiAliasing); +} + +uint8_t CGridPlaneXY::serializeGetVersion() const { return 2; } +void CGridPlaneXY::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_xMin << m_xMax; + out << m_yMin << m_yMax << m_plane_z; + out << m_frequency; + VisualObjectParams_Lines::params_serialize(out); +} + +void CGridPlaneXY::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + { + readFromStreamRender(in); + in >> m_xMin >> m_xMax; + in >> m_yMin >> m_yMax >> m_plane_z; + in >> m_frequency; + if (version == 1) + { + setLineWidth(in.ReadAs()); + enableAntiAliasing(in.ReadAs()); + } + else if (version >= 2) + { + VisualObjectParams_Lines::params_deserialize(in); + } + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +auto CGridPlaneXY::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints({m_xMin, m_yMin, 0}, {m_xMax, m_yMax, 0}); +} diff --git a/libs/viz/src/CGridPlaneXZ.cpp b/libs/viz/src/CGridPlaneXZ.cpp new file mode 100644 index 0000000000..d39fd08843 --- /dev/null +++ b/libs/viz/src/CGridPlaneXZ.cpp @@ -0,0 +1,79 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CGridPlaneXZ, CVisualObject, mrpt::viz) + +/** Constructor */ +CGridPlaneXZ::CGridPlaneXZ( + float xMin, + float xMax, + float zMin, + float zMax, + float y, + float frequency, + float lineWidth, + bool antiAliasing) : + m_xMin(xMin), m_xMax(xMax), m_zMin(zMin), m_zMax(zMax), m_plane_y(y), m_frequency(frequency) +{ + setLineWidth(lineWidth); + enableAntiAliasing(antiAliasing); +} + +uint8_t CGridPlaneXZ::serializeGetVersion() const { return 2; } +void CGridPlaneXZ::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_xMin << m_xMax; + out << m_zMin << m_zMax << m_plane_y; + out << m_frequency; + VisualObjectParams_Lines::params_serialize(out); +} + +void CGridPlaneXZ::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + { + readFromStreamRender(in); + in >> m_xMin >> m_xMax; + in >> m_zMin >> m_zMax >> m_plane_y; + in >> m_frequency; + if (version == 1) + { + setLineWidth(in.ReadAs()); + enableAntiAliasing(in.ReadAs()); + } + else if (version >= 2) + { + VisualObjectParams_Lines::params_deserialize(in); + } + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +auto CGridPlaneXZ::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints({m_xMin, 0, m_zMin}, {m_xMax, 0, m_zMax}); +} diff --git a/libs/viz/src/CMesh.cpp b/libs/viz/src/CMesh.cpp new file mode 100644 index 0000000000..4262964ebc --- /dev/null +++ b/libs/viz/src/CMesh.cpp @@ -0,0 +1,548 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include // First! to avoid conflicts with X.h +// +#include +#include +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::poses; +using namespace mrpt::math; +using namespace std; +using mrpt::img::CImage; + +IMPLEMENTS_SERIALIZABLE(CMesh, CVisualObject, mrpt::viz) + +CMesh::CMesh( + bool enableTransparency, float m_xMin_p, float m_xMax_p, float m_yMin_p, float m_yMax_p) : + m_enableTransparency(enableTransparency), + Z(0, 0), + mask(0, 0), + C(0, 0), + C_r(0, 0), + C_g(0, 0), + C_b(0, 0), + m_xMin(m_xMin_p), + m_xMax(m_xMax_p), + m_yMin(m_yMin_p), + m_yMax(m_yMax_p) +{ + enableTextureLinearInterpolation(true); + enableLight(true); + + setColor_u8(0, 0, 150); +} + +CMesh::~CMesh() = default; + +void CMesh::updateTriangles() const +{ + using mrpt::img::colormap; + + CVisualObject::notifyChange(); + + std::unique_lock lckWrite(m_meshDataMtx.data); + + // Remember: + /** List of triangles in the mesh */ + // mutable + // std::vector + // > actualMesh; + + /** The accumulated normals & counts for each vertex, so normals can be + * averaged. */ + // mutable std::vector > + // vertex_normals; + + const auto cols = Z.cols(); + const auto rows = Z.rows(); + + m_zMin = m_zMax = 0; + + actualMesh.clear(); + if (cols == 0 && rows == 0) return; // empty mesh + + ASSERT_(cols > 0 && rows > 0); + ASSERT_NOT_EQUAL_(m_xMax, m_xMin); + ASSERT_NOT_EQUAL_(m_yMax, m_yMin); + + float normalSign = 1.0f; + if (m_xMax < m_xMin) normalSign *= -1.0f; + if (m_yMax < m_yMin) normalSign *= -1.0f; + + // we have 1 more row & col of vertices than of triangles: + vertex_normals.assign((1 + cols) * (1 + rows), std::pair(TPoint3D(0, 0, 0), 0)); + + if (m_colorFromZ || m_isImage) updateColorsMatrix(); + + bool useMask = false; + if (mask.cols() != 0 && mask.rows() != 0) + { + ASSERT_(mask.cols() == cols && mask.rows() == rows); + useMask = true; + } + const float sCellX = (m_xMax - m_xMin) / (rows - 1); + const float sCellY = (m_yMax - m_yMin) / (cols - 1); + + const auto myColor = getColor_u8(); + + mrpt::viz::TTriangle tri; + + m_zMin = std::numeric_limits::max(); + m_zMax = -std::numeric_limits::max(); + + for (int iX = 0; iX < rows - 1; iX++) + for (int iY = 0; iY < cols - 1; iY++) + { + if (useMask && (!mask(iX, iY) || !mask(iX + 1, iY + 1))) continue; + tri.x(0) = m_xMin + iX * sCellX; + tri.y(0) = m_yMin + iY * sCellY; + tri.z(0) = Z(iX, iY); + tri.x(2) = tri.x(0) + sCellX; + tri.y(2) = tri.y(0) + sCellY; + tri.z(2) = Z(iX + 1, iY + 1); + + // Vertex indices: + TTriangleVertexIndices tvi; + tvi.vind[0] = iX + rows * iY; + tvi.vind[2] = (iX + 1) + rows * (iY + 1); + + // Each quadrangle has up to 2 triangles: + // [0] + // | + // | + // [1]--[2] + // Order: 0,1,2 + if (!useMask || mask(iX + 1, iY)) + { + tri.x(1) = tri.x(2); + tri.y(1) = tri.y(0); + tri.z(1) = Z(iX + 1, iY); + // Assign alpha channel + for (int i = 0; i < 3; i++) tri.a(i) = myColor.A; + + if (m_colorFromZ) + { + mrpt::img::TColorf col(0, 0, 0, 1); + colormap(m_colorMap, C(iX, iY), col.R, col.G, col.B); + tri.r(0) = f2u8(col.R); + tri.g(0) = f2u8(col.G); + tri.b(0) = f2u8(col.B); + colormap(m_colorMap, C(iX + 1, iY), col.R, col.G, col.B); + tri.r(1) = f2u8(col.R); + tri.g(1) = f2u8(col.G); + tri.b(1) = f2u8(col.B); + colormap(m_colorMap, C(iX + 1, iY + 1), col.R, col.G, col.B); + tri.r(2) = f2u8(col.R); + tri.g(2) = f2u8(col.G); + tri.b(2) = f2u8(col.B); + } + else if (m_isImage) + { + if (getTextureImage().isColor()) + { + tri.r(0) = tri.r(1) = tri.r(2) = C_r(iX, iY); + tri.g(0) = tri.g(1) = tri.g(2) = C_g(iX, iY); + tri.b(0) = tri.b(1) = tri.b(2) = C_b(iX, iY); + } + else + { + tri.r(0) = tri.r(1) = tri.r(2) = C(iX, iY); + tri.g(0) = tri.g(1) = tri.g(2) = C(iX, iY); + tri.b(0) = tri.b(1) = tri.b(2) = C(iX, iY); + } + } + else + { + tri.r(0) = tri.r(1) = tri.r(2) = u8tof(myColor.R); + tri.g(0) = tri.g(1) = tri.g(2) = u8tof(myColor.G); + tri.b(0) = tri.b(1) = tri.b(2) = u8tof(myColor.B); + } + + // Compute normal of this triangle, and add it up to the 3 + // neighboring vertices: + // A = P1 - P0, B = P2 - P0 + float ax = tri.x(1) - tri.x(0); + float bx = tri.x(2) - tri.x(0); + float ay = tri.y(1) - tri.y(0); + float by = tri.y(2) - tri.y(0); + float az = tri.z(1) - tri.z(0); + float bz = tri.z(2) - tri.z(0); + const TPoint3D this_normal(ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx); + + // Vertex indices: + tvi.vind[1] = iX + 1 + rows * iY; + + // Add triangle: + actualMesh.emplace_back(tri, tvi); + + // ... and update z bbox: + for (int i = 0; i < 3; i++) + { + mrpt::keep_min(m_zMin, tri.z(i)); + mrpt::keep_max(m_zMax, tri.z(i)); + } + + // For averaging normals: + for (unsigned long k : tvi.vind) + { + vertex_normals[k].first += this_normal * normalSign; + vertex_normals[k].second++; + } + } + // 2: + // [0]--[1->2] + // \ | + // \| + // [2->1] + // Order: 0,2,1 + if (!useMask || mask(iX, iY + 1)) + { + tri.x(1) = tri.x(2); + tri.y(1) = tri.y(2); + tri.z(1) = tri.z(2); + + tri.x(2) = tri.x(0); + // tri.y(2)=tri.y(1); + tri.z(2) = Z(iX, iY + 1); + if (m_colorFromZ) + { + mrpt::img::TColorf col(0, 0, 0, 1); + + colormap(m_colorMap, C(iX, iY), col.R, col.G, col.B); + tri.r(0) = f2u8(col.R); + tri.g(0) = f2u8(col.G); + tri.b(0) = f2u8(col.B); + colormap(m_colorMap, C(iX + 1, iY + 1), col.R, col.G, col.B); + tri.r(1) = f2u8(col.R); + tri.g(1) = f2u8(col.G); + tri.b(1) = f2u8(col.B); + colormap(m_colorMap, C(iX, iY + 1), col.R, col.G, col.B); + tri.r(2) = f2u8(col.R); + tri.g(2) = f2u8(col.G); + tri.b(2) = f2u8(col.B); + } + else if (m_isImage) + { + if (getTextureImage().isColor()) + { + tri.r(0) = tri.r(1) = tri.r(2) = C_r(iX, iY); + tri.g(0) = tri.g(1) = tri.g(2) = C_g(iX, iY); + tri.b(0) = tri.b(1) = tri.b(2) = C_b(iX, iY); + } + else + { + tri.r(0) = tri.r(1) = tri.r(2) = C(iX, iY); + tri.g(0) = tri.g(1) = tri.g(2) = C(iX, iY); + tri.b(0) = tri.b(1) = tri.b(2) = C(iX, iY); + } + } + else + { + tri.r(0) = tri.r(1) = tri.r(2) = u8tof(myColor.R); + tri.g(0) = tri.g(1) = tri.g(2) = u8tof(myColor.G); + tri.b(0) = tri.b(1) = tri.b(2) = u8tof(myColor.B); + } + + // Compute normal of this triangle, and add it up to the 3 + // neighboring vertices: + // A = P1 - P0, B = P2 - P0 + float ax = tri.x(1) - tri.x(0); + float bx = tri.x(2) - tri.x(0); + float ay = tri.y(1) - tri.y(0); + float by = tri.y(2) - tri.y(0); + float az = tri.z(1) - tri.z(0); + float bz = tri.z(2) - tri.z(0); + const TPoint3D this_normal(ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx); + + // Vertex indices: + tvi.vind[1] = tvi.vind[2]; + tvi.vind[2] = iX + rows * (iY + 1); + + // Add triangle: + actualMesh.emplace_back(tri, tvi); + + // ... and update z bbox: + for (int i = 0; i < 3; i++) + { + mrpt::keep_min(m_zMin, tri.z(i)); + mrpt::keep_max(m_zMax, tri.z(i)); + } + + // For averaging normals: + for (unsigned long k : tvi.vind) + { + vertex_normals[k].first += this_normal * normalSign; + vertex_normals[k].second++; + } + } + } + + // Average normals: + for (auto& vertex_normal : vertex_normals) + { + const size_t N = vertex_normal.second; + if (N > 0) + { + vertex_normal.first *= 1.0 / N; + vertex_normal.first = vertex_normal.first.unitarize(); + } + } + + m_trianglesUpToDate = true; + m_polygonsUpToDate = false; +} + +/*--------------------------------------------------------------- + assignImage + ---------------------------------------------------------------*/ +void CMesh::assignImage(const CImage& img) +{ + MRPT_START + + // Make a copy: + VisualObjectParams_TexturedTriangles::assignImage(img); + + // Delete content in Z + Z.setZero(img.getHeight(), img.getWidth()); + + m_modified_Image = true; + m_enableTransparency = false; + m_colorFromZ = false; + m_isImage = true; + m_trianglesUpToDate = false; + + CVisualObject::notifyChange(); + + MRPT_END +} + +void CMesh::assignImageAndZ(const CImage& img, const mrpt::math::CMatrixDynamic& in_Z) +{ + MRPT_START + + // In MRPT<2.7.0, img size must be = to in_z size. + // This condition was removed, and custom texture coordinates added. + + Z = in_Z; + + // Load the texture: + VisualObjectParams_TexturedTriangles::assignImage(img); + + m_modified_Image = true; + m_enableTransparency = false; + m_colorFromZ = false; + m_isImage = true; + m_trianglesUpToDate = false; + + CVisualObject::notifyChange(); + + MRPT_END +} + +uint8_t CMesh::serializeGetVersion() const { return 1; } +void CMesh::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + writeToStreamTexturedObject(out); + + // Version 0: + out << m_xMin << m_xMax << m_yMin << m_yMax; + out << Z << mask; // We don't need to serialize C, it's computed + out << m_enableTransparency; + out << m_colorFromZ; + // new in v1 + out << m_isWireFrame; + out << int16_t(m_colorMap); +} + +void CMesh::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + { + readFromStreamRender(in); + readFromStreamTexturedObject(in); + + in >> m_xMin; + in >> m_xMax; + in >> m_yMin; + in >> m_yMax; + + in >> Z >> mask; + in >> m_enableTransparency; + in >> m_colorFromZ; + + if (version >= 1) + { + in >> m_isWireFrame; + int16_t i; + in >> i; + m_colorMap = mrpt::img::TColormap(i); + } + else + m_isWireFrame = false; + + m_modified_Z = true; + } + m_trianglesUpToDate = false; + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + m_trianglesUpToDate = false; + CVisualObject::notifyChange(); +} + +void CMesh::updateColorsMatrix() const +{ + if ((!m_modified_Z) && (!m_modified_Image)) return; + + CVisualObject::notifyChange(); + + if (m_isImage) + { + const int cols = getTextureImage().getWidth(); + const int rows = getTextureImage().getHeight(); + + if ((cols != Z.cols()) || (rows != Z.rows())) + { + // Texture image and Z matrix have different sizes. + // Don't create color matrices. + } + else if (getTextureImage().isColor()) + { + C_r.setSize(rows, cols); + C_g.setSize(rows, cols); + C_b.setSize(rows, cols); + getTextureImage().getAsRGBMatrices(C_r, C_g, C_b); + } + else + { + C.setSize(rows, cols); + getTextureImage().getAsMatrix(C); + } + } + else + { + const size_t cols = Z.cols(); + const size_t rows = Z.rows(); + C.setSize(rows, cols); + + // Color is proportional to height: + C = Z; + + // If mask is empty -> Normalize the whole mesh + if (mask.empty()) mrpt::math::normalize(C, 0.01f, 0.99f); + + // Else -> Normalize color ignoring masked-out cells: + else + { + float val_max = -std::numeric_limits::max(), + val_min = std::numeric_limits::max(); + bool any_valid = false; + + for (size_t c = 0; c < cols; c++) + for (size_t r = 0; r < rows; r++) + { + if (!mask(r, c)) continue; + any_valid = true; + const float val = C(r, c); + mrpt::keep_max(val_max, val); + mrpt::keep_min(val_min, val); + } + + if (any_valid) + { + float minMaxDelta = val_max - val_min; + if (minMaxDelta == 0) minMaxDelta = 1; + const float minMaxDelta_ = 1.0f / minMaxDelta; + C.array() = (C.array() - val_min) * minMaxDelta_; + } + } + } + + m_modified_Image = false; + m_modified_Z = false; + m_trianglesUpToDate = false; +} + +void CMesh::setZ(const mrpt::math::CMatrixDynamic& in_Z) +{ + Z = in_Z; + m_modified_Z = true; + m_trianglesUpToDate = false; + + // Delete previously loaded images + m_isImage = false; + + CVisualObject::notifyChange(); +} + +void CMesh::setMask(const mrpt::math::CMatrixDynamic& in_mask) +{ + mask = in_mask; + m_trianglesUpToDate = false; + CVisualObject::notifyChange(); +} + +bool CMesh::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ + if (!m_trianglesUpToDate || !m_polygonsUpToDate) updatePolygons(); + return mrpt::math::traceRay(tmpPolys, (o - getCPose()).asTPose(), dist); +} + +mrpt::math::TPolygonWithPlane createPolygonFromTriangle( + const std::pair& p) +{ + math::TPolygon3D tmpPoly(3); + + const mrpt::viz::TTriangle& t = p.first; + for (size_t i = 0; i < 3; i++) tmpPoly[i] = t.vertex(i); + return mrpt::math::TPolygonWithPlane(tmpPoly); +} + +void CMesh::updatePolygons() const +{ + if (!m_trianglesUpToDate) updateTriangles(); + + std::shared_lock lckRead(m_meshDataMtx.data); + size_t N = actualMesh.size(); + tmpPolys.resize(N); + transform(actualMesh.begin(), actualMesh.end(), tmpPolys.begin(), createPolygonFromTriangle); + m_polygonsUpToDate = true; + CVisualObject::notifyChange(); +} + +auto CMesh::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints( + {m_xMin, m_yMin, m_zMin}, {m_xMax, m_yMax, m_zMax}); +} + +void CMesh::adjustGridToImageAR() +{ + ASSERT_(m_isImage); + const float ycenter = 0.5f * (m_yMin + m_yMax); + const float xwidth = m_xMax - m_xMin; + const float newratio = float(getTextureImage().getWidth()) / float(getTextureImage().getHeight()); + m_yMax = ycenter + 0.5f * newratio * xwidth; + m_yMin = ycenter - 0.5f * newratio * xwidth; + CVisualObject::notifyChange(); +} diff --git a/libs/viz/src/CMesh3D.cpp b/libs/viz/src/CMesh3D.cpp new file mode 100644 index 0000000000..db18727330 --- /dev/null +++ b/libs/viz/src/CMesh3D.cpp @@ -0,0 +1,229 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::poses; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CMesh3D, CVisualObject, mrpt::viz) + +CMesh3D::~CMesh3D() = default; + +void CMesh3D::loadMesh( + unsigned int num_verts, + unsigned int num_faces, + int* verts_per_face, + int* face_verts, + float* vert_coords) +{ + // Fill number of vertices for each face + m_is_quad.resize(num_faces); + for (unsigned int i = 0; i < num_faces; i++) + { + if (verts_per_face[i] == 3) + m_is_quad[i] = false; + else if (verts_per_face[i] == 4) + m_is_quad[i] = true; + else + { + THROW_EXCEPTION( + "Incorrect mesh format. It can only be composed of triangles " + "and/or quads"); + } + } + + // Fill the vertices of each face + m_face_verts.resize(num_faces); + unsigned int count = 0; + for (unsigned int f = 0; f < num_faces; f++) + { + m_face_verts[f][0] = face_verts[count++]; + m_face_verts[f][1] = face_verts[count++]; + m_face_verts[f][2] = face_verts[count++]; + if (m_is_quad[f]) + m_face_verts[f][3] = face_verts[count++]; + else + m_face_verts[f][3] = -1; // Meaning it is a triangle + } + + // Fill the 3D coordinates of the vertex + m_vertices.resize(num_verts); + for (unsigned int i = 0; i < num_verts; i++) + { + m_vertices[i][0] = vert_coords[3 * i]; + m_vertices[i][1] = vert_coords[3 * i + 1]; + m_vertices[i][2] = vert_coords[3 * i + 2]; + } + + // Compute the mesh normals (if on) + if (m_computeNormals) + { + m_normals.resize(num_faces); + + for (unsigned int f = 0; f < num_faces; f++) + { + const unsigned int v1 = m_face_verts[f][3]; + const unsigned int v2 = m_face_verts[f][2]; + const unsigned int v3 = m_face_verts[f][1]; + const unsigned int v4 = m_face_verts[f][0]; + + if (m_is_quad[f]) + { + const float vec1[3] = { + m_vertices[v3][0] - m_vertices[v1][0], m_vertices[v3][1] - m_vertices[v1][1], + m_vertices[v3][2] - m_vertices[v1][2]}; + const float vec2[3] = { + m_vertices[v4][0] - m_vertices[v2][0], m_vertices[v4][1] - m_vertices[v2][1], + m_vertices[v4][2] - m_vertices[v2][2]}; + m_normals[f][0] = vec1[1] * vec2[2] - vec1[2] * vec2[1]; + m_normals[f][1] = vec1[2] * vec2[0] - vec1[0] * vec2[2]; + m_normals[f][2] = vec1[0] * vec2[1] - vec1[1] * vec2[0]; + } + else + { + const float vec1[3] = { + m_vertices[v2][0] - m_vertices[v1][0], m_vertices[v2][1] - m_vertices[v1][1], + m_vertices[v2][2] - m_vertices[v1][2]}; + const float vec2[3] = { + m_vertices[v3][0] - m_vertices[v1][0], m_vertices[v3][1] - m_vertices[v1][1], + m_vertices[v3][2] - m_vertices[v1][2]}; + m_normals[f][0] = vec1[1] * vec2[2] - vec1[2] * vec2[1]; + m_normals[f][1] = vec1[2] * vec2[0] - vec1[0] * vec2[2]; + m_normals[f][2] = vec1[0] * vec2[1] - vec1[1] * vec2[0]; + } + m_normals[f] = m_normals[f].unitarize(); + } + } + + CVisualObject::notifyChange(); +} + +void CMesh3D::loadMesh( + unsigned int num_verts, + unsigned int num_faces, + const mrpt::math::CMatrixDynamic& is_quad, + const mrpt::math::CMatrixDynamic& face_verts, + const mrpt::math::CMatrixDynamic& vert_coords) +{ + // Fill number of vertices for each face + m_is_quad.resize(num_faces); + for (unsigned int i = 0; i < num_faces; i++) m_is_quad[i] = is_quad(i, 0); + + // Fill the vertices of each face + m_face_verts.resize(num_faces); + for (unsigned int f = 0; f < num_faces; f++) + { + m_face_verts[f][0] = face_verts(0, f); + m_face_verts[f][1] = face_verts(1, f); + m_face_verts[f][2] = face_verts(2, f); + if (m_is_quad[f]) + m_face_verts[f][3] = face_verts(3, f); + else + m_face_verts[f][3] = -1; // Meaning it is a triangle + } + + // Fill the 3D coordinates of the vertex + m_vertices.resize(num_verts); + for (unsigned int i = 0; i < num_verts; i++) + { + m_vertices[i][0] = vert_coords(0, i); + m_vertices[i][1] = vert_coords(1, i); + m_vertices[i][2] = vert_coords(2, i); + } + + // Compute the mesh normals (if on) + m_normals.resize(num_faces); + if (m_computeNormals) + for (unsigned int f = 0; f < num_faces; f++) + { + const unsigned int v1 = m_face_verts[f][0]; + const unsigned int v2 = m_face_verts[f][1]; + const unsigned int v3 = m_face_verts[f][2]; + const unsigned int v4 = m_face_verts[f][3]; + + if (m_is_quad[f]) + { + const float vec1[3] = { + m_vertices[v3][0] - m_vertices[v1][0], m_vertices[v3][1] - m_vertices[v1][1], + m_vertices[v3][2] - m_vertices[v1][2]}; + const float vec2[3] = { + m_vertices[v4][0] - m_vertices[v2][0], m_vertices[v4][1] - m_vertices[v2][1], + m_vertices[v4][2] - m_vertices[v2][2]}; + m_normals[f][0] = vec1[1] * vec2[2] - vec1[2] * vec2[1]; + m_normals[f][1] = vec1[2] * vec2[0] - vec1[0] * vec2[2]; + m_normals[f][2] = vec1[0] * vec2[1] - vec1[1] * vec2[0]; + } + else + { + const float vec1[3] = { + m_vertices[v2][0] - m_vertices[v1][0], m_vertices[v2][1] - m_vertices[v1][1], + m_vertices[v2][2] - m_vertices[v1][2]}; + const float vec2[3] = { + m_vertices[v3][0] - m_vertices[v1][0], m_vertices[v3][1] - m_vertices[v1][1], + m_vertices[v3][2] - m_vertices[v1][2]}; + m_normals[f][0] = vec1[1] * vec2[2] - vec1[2] * vec2[1]; + m_normals[f][1] = vec1[2] * vec2[0] - vec1[0] * vec2[2]; + m_normals[f][2] = vec1[0] * vec2[1] - vec1[1] * vec2[0]; + } + } + + CVisualObject::notifyChange(); +} + +uint8_t CMesh3D::serializeGetVersion() const { return 1; } +void CMesh3D::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_showEdges << m_showFaces << m_showVertices << m_computeNormals; + out << m_is_quad << m_vertices << m_normals; + out.WriteAs(m_face_verts.size()); + if (!m_face_verts.empty()) + out.WriteBufferFixEndianness( + m_face_verts[0].data(), m_face_verts.size() * m_face_verts[0].size()); + VisualObjectParams_Triangles::params_serialize(out); // v1 +} + +void CMesh3D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + { + readFromStreamRender(in); + in >> m_showEdges >> m_showFaces >> m_showVertices >> m_computeNormals; + in >> m_is_quad >> m_vertices >> m_normals; + const auto N = in.ReadAs(); + m_face_verts.resize(N); + if (!m_face_verts.empty()) + in.ReadBufferFixEndianness( + m_face_verts[0].data(), m_face_verts.size() * m_face_verts[0].size()); + + if (version >= 1) VisualObjectParams_Triangles::params_deserialize(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +auto CMesh3D::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return trianglesBoundingBox(); +} diff --git a/libs/viz/src/CMeshFast.cpp b/libs/viz/src/CMeshFast.cpp new file mode 100644 index 0000000000..d7469e7f53 --- /dev/null +++ b/libs/viz/src/CMeshFast.cpp @@ -0,0 +1,219 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include // First! to avoid conflicts with X.h +// +#include +#include +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::img; +using namespace mrpt::poses; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CMeshFast, CVisualObject, mrpt::viz) + +void CMeshFast::updatePoints() const +{ + CVisualObject::notifyChange(); + + const auto cols = Z.cols(); + const auto rows = Z.rows(); + + if ((m_colorFromZ) || (m_isImage)) updateColorsMatrix(); + + ASSERT_((cols > 0) && (rows > 0)); + ASSERT_((xMax > xMin) && (yMax > yMin)); + X.setSize(rows, cols); + Y.setSize(rows, cols); + const float sCellX = (xMax - xMin) / (rows - 1); + const float sCellY = (yMax - yMin) / (cols - 1); + + for (int iX = 0; iX < rows; iX++) + for (int iY = 0; iY < cols; iY++) + { + X(iX, iY) = xMin + iX * sCellX; + Y(iX, iY) = yMin + iY * sCellY; + } + + pointsUpToDate = true; +} + +void CMeshFast::assignImage(const CImage& img) +{ + MRPT_START + + // Make a copy: + m_textureImage = img; + + // Delete content in Z + Z.setZero(img.getHeight(), img.getWidth()); + + // Update flags/states + m_modified_Image = true; + m_enableTransparency = false; + m_colorFromZ = false; + m_isImage = true; + pointsUpToDate = false; + + CVisualObject::notifyChange(); + + MRPT_END +} + +void CMeshFast::assignImageAndZ(const CImage& img, const mrpt::math::CMatrixDynamic& in_Z) +{ + MRPT_START + + ASSERT_( + (img.getWidth() == static_cast(in_Z.cols())) && + (img.getHeight() == static_cast(in_Z.rows()))); + + Z = in_Z; + + // Make a copy: + m_textureImage = img; + + // Update flags/states + m_modified_Image = true; + m_enableTransparency = false; + m_colorFromZ = false; + m_isImage = true; + pointsUpToDate = false; + + CVisualObject::notifyChange(); + + MRPT_END +} + +uint8_t CMeshFast::serializeGetVersion() const { return 1; } +void CMeshFast::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + + out << m_textureImage; + out << m_isImage; + out << xMin << xMax << yMin << yMax; + out << X << Y << Z; // We don't need to serialize C, it's computed + out << m_enableTransparency; + out << m_colorFromZ; + out << int16_t(m_colorMap); + VisualObjectParams_Points::params_serialize(out); +} + +void CMeshFast::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 1: + { + readFromStreamRender(in); + + in >> m_textureImage; + in >> m_isImage; + + in >> xMin; + in >> xMax; + in >> yMin; + in >> yMax; + + in >> X >> Y >> Z; + in >> m_enableTransparency; + in >> m_colorFromZ; + + int16_t i; + in >> i; + m_colorMap = TColormap(i); + VisualObjectParams_Points::params_deserialize(in); + + m_modified_Z = true; + } + + pointsUpToDate = false; + break; + + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +void CMeshFast::updateColorsMatrix() const +{ + if (!m_modified_Z && !m_modified_Image) return; + + CVisualObject::notifyChange(); + + if (m_isImage) + { + const int cols = m_textureImage.getWidth(); + const int rows = m_textureImage.getHeight(); + + ASSERT_EQUAL_(cols, Z.cols()); + ASSERT_EQUAL_(rows, Z.rows()); + + if (m_textureImage.isColor()) + { + C_r.setSize(rows, cols); + C_g.setSize(rows, cols); + C_b.setSize(rows, cols); + m_textureImage.getAsRGBMatrices(C_r, C_g, C_b); + } + else + { + C.setSize(rows, cols); + m_textureImage.getAsMatrix(C); + } + } + else + { + const size_t cols = Z.cols(); + const size_t rows = Z.rows(); + + C.setSize(rows, cols); + + // Color is proportional to difference between height of a cell and + // the mean of the nearby cells MEANS: + Eigen::MatrixXf Zf = Z.asEigen().cast(); + mrpt::math::normalize(Zf, 0.01f, 0.99f); + + Zf *= 255; + + C = Zf.cast(); + } + + m_modified_Image = false; + m_modified_Z = false; // Done + pointsUpToDate = false; +} + +void CMeshFast::setZ(const mrpt::math::CMatrixDynamic& in_Z) +{ + Z = in_Z; + m_modified_Z = true; + pointsUpToDate = false; + + // Delete previously loaded images + m_isImage = false; + + CVisualObject::notifyChange(); +} + +auto CMeshFast::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return verticesBoundingBox(); +} diff --git a/libs/viz/src/COctoMapVoxels.cpp b/libs/viz/src/COctoMapVoxels.cpp new file mode 100644 index 0000000000..5c1cc352da --- /dev/null +++ b/libs/viz/src/COctoMapVoxels.cpp @@ -0,0 +1,153 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(COctoMapVoxels, CVisualObject, mrpt::viz) + +/** Ctor */ +COctoMapVoxels::COctoMapVoxels() : m_grid_color(0xE0, 0xE0, 0xE0, 0x90) {} +/** Clears everything */ +void COctoMapVoxels::clear() +{ + m_voxel_sets.clear(); + m_grid_cubes.clear(); + + CVisualObject::notifyChange(); +} + +void COctoMapVoxels::setBoundingBox( + const mrpt::math::TPoint3D& bb_min, const mrpt::math::TPoint3D& bb_max) +{ + m_bb_min = bb_min; + m_bb_max = bb_max; +} + +DECLARE_CUSTOM_TTYPENAME(COctoMapVoxels::TInfoPerVoxelSet) +DECLARE_CUSTOM_TTYPENAME(COctoMapVoxels::TGridCube) +DECLARE_CUSTOM_TTYPENAME(COctoMapVoxels::TVoxel) + +namespace mrpt::viz +{ +using mrpt::serialization::CArchive; +CArchive& operator<<(CArchive& out, const COctoMapVoxels::TInfoPerVoxelSet& a) +{ + out << a.visible << a.voxels; + return out; +} +CArchive& operator>>(CArchive& in, COctoMapVoxels::TInfoPerVoxelSet& a) +{ + in >> a.visible >> a.voxels; + return in; +} + +CArchive& operator<<(CArchive& out, const COctoMapVoxels::TGridCube& a) +{ + out << a.min << a.max; + return out; +} +CArchive& operator>>(CArchive& in, COctoMapVoxels::TGridCube& a) +{ + in >> a.min >> a.max; + return in; +} + +CArchive& operator<<(CArchive& out, const COctoMapVoxels::TVoxel& a) +{ + out << a.coords << a.side_length << a.color; + return out; +} +CArchive& operator>>(CArchive& in, COctoMapVoxels::TVoxel& a) +{ + in >> a.coords >> a.side_length >> a.color; + return in; +} +} // end of namespace mrpt::viz + +uint8_t COctoMapVoxels::serializeGetVersion() const { return 4; } +void COctoMapVoxels::serializeTo(CArchive& out) const +{ + writeToStreamRender(out); + + out << m_voxel_sets << m_grid_cubes << m_bb_min << m_bb_max << m_enable_lighting + << m_showVoxelsAsPoints << m_showVoxelsAsPointsSize << m_show_grids << m_grid_width + << m_grid_color << m_enable_cube_transparency // added in v1 + << uint32_t(m_visual_mode); // added in v2 + VisualObjectParams_Triangles::params_serialize(out); // v3 + out.WriteAs(m_color_map); // v4 +} + +void COctoMapVoxels::serializeFrom(CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + case 3: + case 4: + { + readFromStreamRender(in); + + in >> m_voxel_sets >> m_grid_cubes >> m_bb_min >> m_bb_max >> m_enable_lighting >> + m_showVoxelsAsPoints >> m_showVoxelsAsPointsSize >> m_show_grids >> m_grid_width >> + m_grid_color; + + if (version >= 1) + in >> m_enable_cube_transparency; + else + m_enable_cube_transparency = false; + + if (version >= 2) + { + uint32_t i; + in >> i; + m_visual_mode = static_cast(i); + } + else + m_visual_mode = COctoMapVoxels::COLOR_FROM_OCCUPANCY; + + if (version >= 3) VisualObjectParams_Triangles::params_deserialize(in); + + if (version >= 4) m_color_map = static_cast(in.ReadAs()); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + + CVisualObject::notifyChange(); +} + +auto COctoMapVoxels::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints(m_bb_min, m_bb_max); +} + +bool sort_voxels_z(const COctoMapVoxels::TVoxel& a, const COctoMapVoxels::TVoxel& b) +{ + return a.coords.z < b.coords.z; +} + +void COctoMapVoxels::sort_voxels_by_z() +{ + for (auto& m_voxel_set : m_voxel_sets) + { + std::sort(m_voxel_set.voxels.begin(), m_voxel_set.voxels.end(), &sort_voxels_z); + } +} diff --git a/libs/viz/src/CPointCloud.cpp b/libs/viz/src/CPointCloud.cpp new file mode 100644 index 0000000000..41c824b7e8 --- /dev/null +++ b/libs/viz/src/CPointCloud.cpp @@ -0,0 +1,305 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include // round() +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::img; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CPointCloud, CVisualObject, mrpt::viz) + +CPointCloud::CPointCloud() { markAllPointsAsNew(); } + +void CPointCloud::serializeTo(mrpt::serialization::CSchemeArchiveBase& out) const +{ + SCHEMA_SERIALIZE_DATATYPE_VERSION(1); + out["colorFromDepth"] = static_cast(m_colorFromDepth); + out["pointSize"] = getPointSize(); + const auto N = m_points.size(); + out["N"] = static_cast(N); + for (size_t i = 0; i < N; i++) + { + out["xs"][i] = m_points[i].x; + out["ys"][i] = m_points[i].y; + out["zs"][i] = m_points[i].z; + } + out["colorFromDepth_min"]["R"] = m_colorFromDepth_min.R; + out["colorFromDepth_min"]["G"] = m_colorFromDepth_min.G; + out["colorFromDepth_min"]["B"] = m_colorFromDepth_min.B; + out["colorFromDepth_max"]["R"] = m_colorFromDepth_max.R; + out["colorFromDepth_max"]["G"] = m_colorFromDepth_max.G; + out["colorFromDepth_max"]["B"] = m_colorFromDepth_max.B; +} +void CPointCloud::serializeFrom(mrpt::serialization::CSchemeArchiveBase& in) +{ + uint8_t version; + SCHEMA_DESERIALIZE_DATATYPE_VERSION(); + switch (version) + { + case 1: + { + /** + * currently below is being left to what is being set by + * the default constructor i.e.,CPointCloud::colNone + */ + // m_colorFromDepth = static_cast(in["colorDepth"]); + setPointSize(static_cast(in["pointSize"])); + const size_t N = static_cast(in["N"]); + m_points.resize(N); + for (size_t i = 0; i < N; i++) + { + m_points[i].x = static_cast(in["xs"][i]); + m_points[i].y = static_cast(in["ys"][i]); + m_points[i].z = static_cast(in["zs"][i]); + } + m_colorFromDepth_min.R = static_cast(in["colorFromDepth_min"]["R"]); + m_colorFromDepth_min.G = static_cast(in["colorFromDepth_min"]["G"]); + m_colorFromDepth_min.B = static_cast(in["colorFromDepth_min"]["B"]); + m_colorFromDepth_max.R = static_cast(in["colorFromDepth_max"]["R"]); + m_colorFromDepth_max.G = static_cast(in["colorFromDepth_max"]["G"]); + m_colorFromDepth_max.B = static_cast(in["colorFromDepth_max"]["B"]); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + } +} +uint8_t CPointCloud::serializeGetVersion() const { return 7; } +void CPointCloud::serializeTo(mrpt::serialization::CArchive& out) const +{ + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + + writeToStreamRender(out); + // Changed from bool to enum/int32_t in version 3. + out.WriteAs(m_colorFromDepth); + + // out << m_xs << m_ys << m_zs;// was: v4 + out.WriteAs(m_points.size()); + for (const auto& pt : m_points) out << pt; + + // New in version 2: + out << m_colorFromDepth_min.R << m_colorFromDepth_min.G << m_colorFromDepth_min.B; + out << m_colorFromDepth_max.R << m_colorFromDepth_max.G << m_colorFromDepth_max.B; + + // new v6: + VisualObjectParams_Points::params_serialize(out); +} + +void CPointCloud::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + switch (version) + { + case 0: + case 1: + case 2: + case 3: + case 4: + case 5: + case 6: + case 7: + { + readFromStreamRender(in); + if (version >= 3) + { + int32_t axis; + in >> axis; + m_colorFromDepth = Axis(axis); + } + else + { + bool colorFromZ; + in >> colorFromZ; + m_colorFromDepth = colorFromZ ? CPointCloud::colZ : CPointCloud::colNone; + } + if (version < 5) + { + std::vector xs, ys, zs; + in >> xs >> ys >> zs; + wfWriteLock.unlock(); // avoid recursive lock + this->setAllPoints(xs, ys, zs); + wfWriteLock.lock(); + } + else + { + // New in v5: + auto N = in.ReadAs(); + m_points.resize(N); + for (auto& pt : m_points) in >> pt; + } + + if (version >= 1 && version < 6) setPointSize(in.ReadAs()); + + if (version >= 2) + { + in >> m_colorFromDepth_min.R >> m_colorFromDepth_min.G >> m_colorFromDepth_min.B; + in >> m_colorFromDepth_max.R >> m_colorFromDepth_max.G >> m_colorFromDepth_max.B; + } + else + { + m_colorFromDepth_min = TColorf(0, 0, 0); + m_colorFromDepth_max = getColor(); + } + + if (version >= 4 && version < 7) + { + bool m_pointSmooth_ignored; + in >> m_pointSmooth_ignored; + } + + if (version >= 6) VisualObjectParams_Points::params_deserialize(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); +} + +void CPointCloud::clear() +{ + { + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + m_points.clear(); + } + + markAllPointsAsNew(); + CVisualObject::notifyChange(); +} + +void CPointCloud::insertPoint(float x, float y, float z) +{ + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + m_points.emplace_back(x, y, z); + + m_minmax_valid = false; + + // JL: TODO note: Well, this can be clearly done much more efficiently + // but...I don't have time! :-( + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); +} + +/** Write an individual point (checks for "i" in the valid range only in + * Debug). + */ +void CPointCloud::setPoint(size_t i, const float x, const float y, const float z) +{ + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + m_points.at(i) = {x, y, z}; + + m_minmax_valid = false; + + // JL: TODO note: Well, this can be clearly done much more efficiently + // but...I don't have time! :-( + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); +} + +/*--------------------------------------------------------------- + setGradientColors +---------------------------------------------------------------*/ +void CPointCloud::setGradientColors( + const mrpt::img::TColorf& colorMin, const mrpt::img::TColorf& colorMax) +{ + m_colorFromDepth_min = colorMin; + m_colorFromDepth_max = colorMax; +} + +// Do needed internal work if all points are new (octree rebuilt,...) +void CPointCloud::markAllPointsAsNew() +{ + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + m_minmax_valid = false; + CVisualObject::notifyChange(); +} + +/** In a base class, reserve memory to prepare subsequent calls to + * PLY_import_set_vertex */ +void CPointCloud::PLY_import_set_vertex_count(size_t N) { this->resize(N); } + +/** In a base class, will be called after PLY_import_set_vertex_count() once + * for each loaded point. \param pt_color Will be nullptr if the loaded file + * does not provide color info. + */ +void CPointCloud::PLY_import_set_vertex( + size_t idx, + const mrpt::math::TPoint3Df& pt, + [[maybe_unused]] const mrpt::img::TColorf* pt_color) +{ + this->setPoint(idx, pt.x, pt.y, pt.z); +} + +/** In a base class, return the number of vertices */ +size_t CPointCloud::PLY_export_get_vertex_count() const { return this->size(); } +/** In a base class, will be called after PLY_export_get_vertex_count() once + * for each exported point. \param pt_color Will be nullptr if the loaded + * file does not provide color info. + */ +void CPointCloud::PLY_export_get_vertex( + size_t idx, + mrpt::math::TPoint3Df& pt, + bool& pt_has_color, + [[maybe_unused]] mrpt::img::TColorf& pt_color) const +{ + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + + pt_has_color = false; + + pt = m_points[idx]; +} + +TBoundingBoxf CPointCloud::internalBoundingBoxLocal() const +{ + // m_pointsMtx.data: already held by calls inside. + if (empty()) return {}; + auto bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); + for (const auto& pt : m_points) bb.updateWithPoint(pt); + + return bb; +} + +void CPointCloud::setAllPoints(const std::vector& pts) +{ + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + const auto N = pts.size(); + m_points.resize(N); + for (size_t i = 0; i < N; i++) m_points[i] = pts[i]; + m_minmax_valid = false; + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); +} + +void CPointCloud::toYAMLMap(mrpt::containers::yaml& propertiesMap) const +{ + CVisualObject::toYAMLMap(propertiesMap); + propertiesMap["point_count"] = m_points.size(); +} diff --git a/libs/viz/src/CPointCloudColoured.cpp b/libs/viz/src/CPointCloudColoured.cpp new file mode 100644 index 0000000000..cf732b3642 --- /dev/null +++ b/libs/viz/src/CPointCloudColoured.cpp @@ -0,0 +1,209 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include // round() +#include // for << ops +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; +using mrpt::serialization::CArchive; + +IMPLEMENTS_SERIALIZABLE(CPointCloudColoured, CVisualObject, mrpt::viz) + +uint8_t CPointCloudColoured::serializeGetVersion() const { return 4; } +void CPointCloudColoured::serializeTo(mrpt::serialization::CArchive& out) const +{ + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + + writeToStreamRender(out); + out << m_points << m_point_colors; + VisualObjectParams_Points::params_serialize(out); +} + +void CPointCloudColoured::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + switch (version) + { + case 0: + case 1: + case 2: + case 3: + { + THROW_EXCEPTION("Binary backward compatibility lost for this class."); + } + break; + case 4: + { + readFromStreamRender(in); + in >> m_points >> m_point_colors; + + VisualObjectParams_Points::params_deserialize(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); +} + +/** Write an individual point (checks for "i" in the valid range only in Debug). + */ +void CPointCloudColoured::setPoint(size_t i, const TPointXYZfRGBAu8& p) +{ + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + +#ifdef _DEBUG + ASSERT_LT_(i, size_unprotected()); +#endif + m_points[i] = p.pt; + auto& c = m_point_colors[i]; + c.R = p.r; + c.G = p.g; + c.B = p.b; + c.A = p.a; + + // JL: TODO note: Well, this can be clearly done much more efficiently + // but...I don't have time! :-( + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); +} + +/** Inserts a new point into the point cloud. */ +void CPointCloudColoured::push_back(float x, float y, float z, float R, float G, float B, float A) +{ + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + m_points.emplace_back(x, y, z); + m_point_colors.emplace_back(f2u8(R), f2u8(G), f2u8(B), f2u8(A)); + + // JL: TODO note: Well, this can be clearly done much more efficiently + // but...I don't have time! :-( + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); +} + +void CPointCloudColoured::insertPoint(const mrpt::math::TPointXYZfRGBAu8& p) +{ + std::unique_lock wfWriteLock(VisualObjectParams_Points::m_pointsMtx.data); + + m_points.emplace_back(p.pt); + m_point_colors.emplace_back(p.r, p.g, p.b, p.a); + + wfWriteLock.unlock(); + markAllPointsAsNew(); + CVisualObject::notifyChange(); +} + +// Do needed internal work if all points are new (octree rebuilt,...) +void CPointCloudColoured::markAllPointsAsNew() { CVisualObject::notifyChange(); } + +mrpt::math::TBoundingBoxf CPointCloudColoured::internalBoundingBoxLocal() const +{ + // m_pointsMtx.data: already held by calls inside. + if (empty()) return {}; + auto bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); + for (const auto& pt : m_points) bb.updateWithPoint(pt); + + return bb; +} + +/** In a base class, reserve memory to prepare subsequent calls to + * PLY_import_set_vertex */ +void CPointCloudColoured::PLY_import_set_vertex_count(size_t N) { this->resize(N); } + +/** In a base class, will be called after PLY_import_set_vertex_count() once for + * each loaded point. + * \param pt_color Will be nullptr if the loaded file does not provide color + * info. + */ +void CPointCloudColoured::PLY_import_set_vertex( + size_t idx, const mrpt::math::TPoint3Df& pt, const mrpt::img::TColorf* pt_color) +{ + if (!pt_color) + this->setPoint(idx, TPointXYZfRGBAu8(pt.x, pt.y, pt.z, 0xff, 0xff, 0xff)); + else + this->setPoint( + idx, TPointXYZfRGBAu8( + pt.x, pt.y, pt.z, f2u8(pt_color->R), f2u8(pt_color->G), f2u8(pt_color->B))); +} + +/** In a base class, return the number of vertices */ +size_t CPointCloudColoured::PLY_export_get_vertex_count() const { return this->size(); } + +/** In a base class, will be called after PLY_export_get_vertex_count() once for + * each exported point. + * \param pt_color Will be nullptr if the loaded file does not provide color + * info. + */ +void CPointCloudColoured::PLY_export_get_vertex( + size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color, mrpt::img::TColorf& pt_color) const +{ + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + + auto& p = m_points[idx]; + auto& p_color = m_point_colors[idx]; + p = pt; + p_color = pt_color.asTColor(); + pt_has_color = true; +} + +void CPointCloudColoured::recolorizeByCoordinate( + const float coord_min, + const float coord_max, + const int coord_index, + const mrpt::img::TColormap color_map) +{ + ASSERT_GE_(coord_index, 0); + ASSERT_LT_(coord_index, 3); + + const float coord_range = coord_max - coord_min; + const float coord_range_1 = coord_range != 0.0f ? 1.0f / coord_range : 1.0f; + for (size_t i = 0; i < m_points.size(); i++) + { + float coord = .0f; + switch (coord_index) + { + case 0: + coord = m_points[i].x; + break; + case 1: + coord = m_points[i].y; + break; + case 2: + coord = m_points[i].z; + break; + }; + const float col_idx = std::max(0.0f, std::min(1.0f, (coord - coord_min) * coord_range_1)); + float r, g, b; + mrpt::img::colormap(color_map, col_idx, r, g, b); + this->setPointColor_fast(i, r, g, b); + } +} + +void CPointCloudColoured::toYAMLMap(mrpt::containers::yaml& propertiesMap) const +{ + CVisualObject::toYAMLMap(propertiesMap); + propertiesMap["point_count"] = m_points.size(); + propertiesMap["bounding_box"] = this->getBoundingBox().asString(); +} diff --git a/libs/viz/src/CPolyhedron.cpp b/libs/viz/src/CPolyhedron.cpp new file mode 100644 index 0000000000..257ea3c213 --- /dev/null +++ b/libs/viz/src/CPolyhedron.cpp @@ -0,0 +1,2120 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include +#include +#include // dotProduct() +#include +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::math; +using namespace mrpt::viz; +using namespace mrpt::poses; +using namespace std; +using mrpt::serialization::CArchive; + +IMPLEMENTS_SERIALIZABLE(CPolyhedron, CVisualObject, mrpt::viz) + +// Auxiliary data and code +template +class FCreatePolygonFromFace +{ + public: + const vector& verts; + FCreatePolygonFromFace(const vector& v) : verts(v) {} + TPolygon3D p; + T operator()(const CPolyhedron::TPolyhedronFace& f) + { + p = TPolygon3D(f.vertices.size()); + for (size_t i = 0; i < f.vertices.size(); i++) p[i] = verts[f.vertices[i]]; + return p; + } +}; + +bool getVerticesAndFaces( + const vector& polys, + vector& vertices, + vector& faces) +{ + vertices.reserve(4 * polys.size()); + faces.reserve(polys.size()); + for (const auto& poly : polys) + { + size_t N = poly.size(); + if (N < 3) return false; + CPolyhedron::TPolyhedronFace f; + f.vertices.resize(N); + for (size_t i = 0; i < N; i++) + { + auto it2 = find(vertices.begin(), vertices.end(), poly[i]); + if (it2 == vertices.end()) + { + f.vertices[i] = vertices.size(); + vertices.push_back(poly[i]); + } + else + f.vertices[i] = it2 - vertices.begin(); + } + faces.push_back(f); + } + return true; +} + +enum JohnsonBodyPart +{ + INF_NO_BODY = -2, + SUP_NO_BODY = -1, + UPWARDS_PYRAMID = 0, + DOWNWARDS_PYRAMID = 1, + UPWARDS_CUPOLA = 2, + DOWNWARDS_CUPOLA = 3, + ROTATED_UPWARDS_CUPOLA = 4, + ROTATED_DOWNWARDS_CUPOLA = 5, + PRISM = 6, + ANTIPRISM = 7, + UPWARDS_ROTUNDA = 8, + DOWNWARDS_ROTUNDA = 9, + ROTATED_UPWARDS_ROTUNDA = 10, + ROTATED_DOWNWARDS_ROTUNDA = 11 +}; +bool analyzeJohnsonPartsString( + const std::string& components, uint32_t numBaseEdges, vector& parts) +{ + size_t N = components.length(); + size_t i = 0; + bool rot = false; + while (i < N) + { + switch (components[i]) + { + case 'A': + case 'a': + if (parts.size() == 0) parts.push_back(INF_NO_BODY); + parts.push_back(ANTIPRISM); + break; + case 'C': + case 'c': + if (numBaseEdges & 1) return false; + if (i == N - 1) return false; + i++; + if (components[i] == '+') + { + if (i != N - 1) return false; + if (parts.size() == 0) parts.push_back(INF_NO_BODY); + parts.push_back(rot ? ROTATED_UPWARDS_CUPOLA : UPWARDS_CUPOLA); + rot = false; + } + else if (components[i] == '-') + { + if (parts.size() > 0) return false; + parts.push_back(rot ? ROTATED_DOWNWARDS_CUPOLA : DOWNWARDS_CUPOLA); + rot = false; + } + else + return false; + break; + case 'R': + case 'r': + if (numBaseEdges != 10) return false; + if (i == N - 1) return false; + i++; + if (components[i] == '+') + { + if (i != N - 1) return false; + if (parts.size() == 0) parts.push_back(INF_NO_BODY); + parts.push_back(rot ? ROTATED_UPWARDS_ROTUNDA : UPWARDS_ROTUNDA); + rot = false; + } + else if (components[i] == '-') + { + if (parts.size() > 0) return false; + parts.push_back(rot ? ROTATED_DOWNWARDS_ROTUNDA : DOWNWARDS_ROTUNDA); + rot = false; + } + else + return false; + break; + case 'G': + case 'g': + if (i == N - 1) return false; + i++; + if (components[i] == 'C' || components[i] == 'R') + { + rot = true; + continue; + } + else + return false; + case 'P': + case 'p': + if (i == N - 1) return false; + i++; + switch (components[i]) + { + case '+': + if (numBaseEdges > 5) return false; + if (i != N - 1) return false; + if (parts.size() == 0) parts.push_back(INF_NO_BODY); + parts.push_back(UPWARDS_PYRAMID); + break; + case '-': + if (numBaseEdges > 5) return false; + if (i != 1) return false; + parts.push_back(DOWNWARDS_PYRAMID); + break; + case 'R': + case 'r': + if (parts.size() > 0 && (*parts.rbegin() == PRISM)) return false; + if (parts.size() == 0) parts.push_back(INF_NO_BODY); + parts.push_back(PRISM); + break; + default: + return false; + } + break; + default: + return false; + } + i++; + } + if (parts.size() == 0) return false; + JohnsonBodyPart p = *parts.rbegin(); + if (p != UPWARDS_PYRAMID && p != UPWARDS_CUPOLA && p != ROTATED_UPWARDS_CUPOLA && + p != UPWARDS_ROTUNDA && p != ROTATED_UPWARDS_ROTUNDA) + parts.push_back(SUP_NO_BODY); + return true; +} +size_t additionalVertices(JohnsonBodyPart j, uint32_t numBaseEdges) +{ + if (j == INF_NO_BODY || j == SUP_NO_BODY) + return 0; + else if (j < UPWARDS_CUPOLA) + return 1; // j is a pyramid + else if (j < PRISM) + return numBaseEdges >> 1; // j is a cupola + else if (j < UPWARDS_ROTUNDA) + return numBaseEdges; // j is a prism or antiprism + else + return 10; // j is a rotunda +} +void insertCupola( + size_t numBaseEdges, + double angleShift, + double baseRadius, + double edgeLength, + bool isRotated, + bool isUpwards, + size_t base, + vector& verts, + vector& faces) +{ + size_t edges2 = numBaseEdges >> 1; + double minorRadius = baseRadius * sin(M_PI / numBaseEdges) / sin(M_PI / edges2); + //"Proper base"'s apothem=base radius*cos(2*pi/(2*numBaseEdges)) + //"Small base"'s apothem=small radius*cos(2*pi/2*(numBaseEdges/2)) + // Cupola's height is so that (Da^2+Height^2=Edge length^2, where Da is the + // difference between both apothems. + double h = sqrt( + square(edgeLength) - + square(baseRadius * cos(M_PI / numBaseEdges) - minorRadius * cos(M_PI / edges2))); + double height = verts[base].z + (isUpwards ? h : -h); + angleShift += M_PI / edges2 + (isRotated ? -M_PI / numBaseEdges : M_PI / numBaseEdges); + size_t minorBase = verts.size(); + for (size_t i = 0; i < edges2; i++) + { + double ang = angleShift + 2 * M_PI * i / edges2; + verts.emplace_back(minorRadius * cos(ang), minorRadius * sin(ang), height); + } + CPolyhedron::TPolyhedronFace tri, quad, cBase; + tri.vertices.resize(3); + quad.vertices.resize(4); + cBase.vertices.resize(edges2); + size_t iq = isRotated ? 1 : 2, it = 0; + for (size_t i = 0; i < edges2; i++) + { + cBase.vertices[i] = it + minorBase; + size_t iiq = (iq + 1) % numBaseEdges + base; + size_t iiiq = (iiq + 1) % numBaseEdges + base; + size_t iit = (it + 1) % edges2 + minorBase; + quad.vertices[0] = it + minorBase; + quad.vertices[1] = iit; + quad.vertices[2] = iiq; + quad.vertices[3] = iq + base; + tri.vertices[0] = iit; + tri.vertices[1] = iiq; + tri.vertices[2] = iiiq; + iq = (iq + 2) % numBaseEdges; + it = (it + 1) % edges2; + faces.push_back(tri); + faces.push_back(quad); + } + if (edges2 >= 3) faces.push_back(cBase); +} +void insertRotunda( + double angleShift, + double baseRadius, + bool isRotated, + bool isUpwards, + size_t base, + vector& verts, + vector& faces) +{ + double R1 = baseRadius * sqrt((5.0 - sqrt(5.0)) / 10.0); + double R2 = baseRadius * sqrt((5.0 + sqrt(5.0)) / 10.0); + double baseHeight = verts[base].z; + TPoint3D p1[5], p2[5]; + angleShift += M_PI / 10; + if (isRotated) angleShift += M_PI / 5; + for (size_t i = 0; i < 5; i++) + { + double a = (i + i + 1) * M_PI / 5 + angleShift; + double b = (i + i) * M_PI / 5 + angleShift; + double ca = cos(a), sa = sin(a), cb = cos(b), sb = sin(b); + p1[i].x = R1 * ca; + p1[i].y = R1 * sa; + p1[i].z = baseHeight + (isUpwards ? R2 : -R2); + p2[i].x = R2 * cb; + p2[i].y = R2 * sb; + p2[i].z = baseHeight + (isUpwards ? R1 : -R1); + } + size_t newBase = verts.size(); + for (const auto& i : p1) verts.push_back(i); + for (const auto& i : p2) verts.push_back(i); + CPolyhedron::TPolyhedronFace f, g; + f.vertices.resize(3); + g.vertices.resize(5); + size_t baseStart = isRotated ? 2 : 1; + for (size_t i = 0; i < 5; i++) + { + size_t ii = (i + 1) % 5; + f.vertices[0] = newBase + i; + f.vertices[1] = newBase + ii; + f.vertices[2] = newBase + ii + 5; + faces.push_back(f); + f.vertices[0] = newBase + i + 5; + f.vertices[1] = ((i + i + baseStart) % 10) + base; + f.vertices[2] = ((i + i + 9 + baseStart) % 10) + base; + faces.push_back(f); + g.vertices[0] = newBase + (ii % 5) + 5; + g.vertices[1] = newBase + i; + g.vertices[2] = newBase + i + 5; + g.vertices[3] = (i + i + baseStart) % 10 + base; + g.vertices[4] = (i + i + baseStart + 1) % 10 + base; + faces.push_back(g); + } + for (size_t i = 0; i < 5; i++) g.vertices[i] = i + newBase; + faces.push_back(g); + return; +} +size_t additionalFaces(JohnsonBodyPart j, uint32_t numBaseEdges) +{ + if (j == INF_NO_BODY || j == SUP_NO_BODY) + return 1; // j is a base + else if (j < UPWARDS_CUPOLA) + return numBaseEdges; // j is a pyramid + else if (j < PRISM) + return numBaseEdges + ((numBaseEdges >= 6) ? 1 : 0); // j is a cupola + else if (j == PRISM) + return numBaseEdges; + else if (j == ANTIPRISM) + return numBaseEdges << 1; + else + return 16; // j is a rotunda +} + +bool faceContainsEdge(const CPolyhedron::TPolyhedronFace& f, const CPolyhedron::TPolyhedronEdge& e) +{ + char hm = 0; + for (unsigned int vertice : f.vertices) + if (vertice == e.v1 || vertice == e.v2) hm++; + return hm == 2; +} + +bool getPlanesIntersection(const vector& planes, TPoint3D& pnt) +{ + if (planes.size() < 3) return false; + char o = 0; + TPlane pl = *planes[0]; + TLine3D l; + TObject3D obj; + for (size_t i = 1; i < planes.size(); i++) switch (o) + { + case 0: + if (!intersect(pl, *planes[i], obj)) return false; + if (obj.getPlane(pl)) + o = 0; + else if (obj.getLine(l)) + o = 1; + else if (obj.getPoint(pnt)) + o = 2; + else + return false; + break; + case 1: + if (!intersect(l, *planes[i], obj)) return false; + if (obj.getLine(l)) + o = 1; + else if (obj.getPoint(pnt)) + o = 2; + else + return false; + break; + case 2: + if (!planes[i]->contains(pnt)) return false; + break; + default: + return false; + } + return o == 2; +} + +bool searchForFace( + const vector& fs, uint32_t v1, uint32_t v2, uint32_t v3) +{ + for (const auto& it : fs) + { + const vector& f = it.vertices; + size_t hmf = 0; + for (unsigned int it2 : f) + { + if (it2 == v1) + hmf |= 1; + else if (it2 == v2) + hmf |= 2; + else if (it2 == v3) + hmf |= 4; + } + if (hmf == 7) return true; + } + return false; +} +bool searchForEdge( + const vector& es, uint32_t v1, uint32_t v2, size_t& where) +{ + for (where = 0; where < es.size(); where++) + { + const CPolyhedron::TPolyhedronEdge& e = es[where]; + if (e.v1 == v1 && e.v2 == v2) + return true; + else if (e.v1 == v2 && e.v2 == v1) + return false; + } + throw std::logic_error("Internal error. Edge not found"); +} +bool searchForEdge( + const vector::const_iterator& begin, + const vector::const_iterator& end, + uint32_t v1, + uint32_t v2) +{ + for (vector::const_iterator it = begin; it != end; ++it) + { + const vector& f = it->vertices; + char res = 0; + for (unsigned int it2 : f) + if (it2 == v1) + res |= 1; + else if (it2 == v2) + res |= 2; + if (res == 3) return true; + } + return false; +} +double getHeight(const TPolygon3D& p, const TPoint3D& c) +{ + size_t N = p.size(); + if (N > 5 || N < 3) throw std::logic_error("Faces must have exactly 3, 4 or 5 vertices."); + double r = mrpt::math::distance(p[0], c); + double l = mrpt::math::distance(p[0], p[1]); + for (size_t i = 1; i < N; i++) + if (std::abs(mrpt::math::distance(p[i], c) - r) >= mrpt::math::getEpsilon()) + throw std::logic_error("There is a non-regular polygon."); + else if (std::abs(mrpt::math::distance(p[i], p[(i + 1) % N]) - l) >= mrpt::math::getEpsilon()) + throw std::logic_error("There is a non-regular polygon."); + return sqrt(square(l) - square(r)); +} +struct SegmentVector +{ + double d[3]; + double mod; + double& operator[](size_t i) { return d[i]; } + double operator[](size_t i) const { return d[i]; } +}; +// End of auxiliary data and code + +double CPolyhedron::TPolyhedronEdge::length(const vector& vertices) const +{ + return mrpt::math::distance(vertices[v1], vertices[v2]); +} + +double CPolyhedron::TPolyhedronFace::area(const vector& vs) const +{ + // Calculate as fan of triangles. + size_t N = vertices.size(); + vector d(N - 1); + for (size_t i = 1; i < N; i++) + { + d[i - 1].mod = 0; + for (size_t j = 0; j < 3; j++) + { + d[i - 1][j] = vs[vertices[i]][j] - vs[vertices[0]][j]; + d[i - 1].mod += square(d[i - 1][j]); + } + d[i - 1].mod = sqrt(d[i - 1].mod); + } + double res = 0; + for (size_t i = 1; i < N - 1; i++) + res += sqrt(square(d[i - 1].mod * d[i].mod) - square(dotProduct<3, double>(d[i - 1], d[i]))); + return res / 2; +} + +void CPolyhedron::TPolyhedronFace::getCenter(const vector& vrts, TPoint3D& p) const +{ + p.x = p.y = p.z = 0.0; + for (unsigned int vertice : vertices) + { + p.x += vrts[vertice].x; + p.y += vrts[vertice].y; + p.z += vrts[vertice].z; + } + size_t N = vertices.size(); + p.x /= N; + p.y /= N; + p.z /= N; +} + +CPolyhedron::CPolyhedron(const std::vector& polys) : + m_Edges(), m_Wireframe(false), polygonsUpToDate(false) +{ + std::vector vertices(0); + std::vector faces; + if (!getVerticesAndFaces(polys, vertices, faces)) throw std::logic_error("Can't create CPolygon"); + m_Vertices = std::move(vertices); + m_Faces = std::move(faces); + + InitFromVertAndFaces(m_Vertices, m_Faces); +} + +CPolyhedron::CPolyhedron(const vector& vertices, const vector>& faces) +{ + vector aux; + for (const auto& face : faces) + { + TPolyhedronFace f; + f.vertices = face; + aux.push_back(f); + } + InitFromVertAndFaces(vertices, aux); +} + +CPolyhedron::Ptr CPolyhedron::CreateCubicPrism( + double x1, double x2, double y1, double y2, double z1, double z2) +{ + vector verts; + vector faces; + for (int i = 0; i < 8; i++) + verts.emplace_back((i & 1) ? x2 : x1, (i & 2) ? y2 : y1, (i & 4) ? z2 : z1); + static uint32_t faceVertices[] = {0, 1, 5, 4, 2, 3, 7, 6, 0, 2, 6, 4, + 1, 3, 7, 5, 0, 1, 3, 2, 4, 5, 7, 6}; + TPolyhedronFace f; + for (auto* p = reinterpret_cast(&faceVertices); + p < 24 + reinterpret_cast(&faceVertices); p += 4) + { + f.vertices.insert(f.vertices.begin(), p, p + 4); + faces.push_back(f); + f.vertices.clear(); + } + return CreateNoCheck(verts, faces); +} + +CPolyhedron::Ptr CPolyhedron::CreatePyramid(const vector& baseVertices, double height) +{ + uint32_t n = baseVertices.size(); + if (baseVertices.size() < 3) throw std::logic_error("Not enough vertices"); + vector verts; + vector faces; + verts.emplace_back(0, 0, height); + for (auto baseVertice : baseVertices) verts.emplace_back(baseVertice.x, baseVertice.y, 0); + TPolyhedronFace f, g; + f.vertices.push_back(0); + f.vertices.push_back(n); + f.vertices.push_back(1); + g.vertices.push_back(1); + faces.push_back(f); + for (uint32_t i = 2; i <= n; i++) + { + f.vertices.erase(f.vertices.begin() + 1); + f.vertices.push_back(i); + faces.push_back(f); + g.vertices.push_back(i); + } + faces.push_back(g); + return CreateNoCheck(verts, faces); +} + +CPolyhedron::Ptr CPolyhedron::CreateDoublePyramid( + const vector& baseVertices, double height1, double height2) +{ + uint32_t N = baseVertices.size(); + if (N < 3) throw std::logic_error("Not enough vertices"); + vector verts; + verts.reserve(N + 2); + vector faces; + faces.reserve(N << 1); + verts.emplace_back(0, 0, height1); + for (auto baseVertice : baseVertices) verts.emplace_back(baseVertice.x, baseVertice.y, 0); + verts.emplace_back(0, 0, -height2); + TPolyhedronFace f, g; + f.vertices.resize(3); + g.vertices.resize(3); + f.vertices[0] = 0; + g.vertices[0] = N + 1; + for (uint32_t i = 1; i < N; i++) + { + f.vertices[1] = g.vertices[1] = i; + f.vertices[2] = g.vertices[2] = i + 1; + faces.push_back(f); + faces.push_back(g); + } + f.vertices[1] = g.vertices[1] = 1; + faces.push_back(f); + faces.push_back(g); + return CreateNoCheck(verts, faces); +} + +CPolyhedron::Ptr CPolyhedron::CreateTruncatedPyramid( + const vector& baseVertices, double height, double ratio) +{ + uint32_t n = baseVertices.size(); + if (n < 3) throw std::logic_error("Not enough vertices"); + vector verts(n + n); + vector faces(n + 2); + TPolyhedronFace f, g, h; + f.vertices.resize(4); + g.vertices.resize(n); + h.vertices.resize(n); + for (uint32_t i = 0; i < n; i++) + { + verts[i] = TPoint3D(baseVertices[i].x, baseVertices[i].y, 0); + verts[i + n] = TPoint3D(baseVertices[i].x * ratio, baseVertices[i].y * ratio, height); + uint32_t ii = (i + 1) % n; + f.vertices[0] = i; + f.vertices[1] = ii; + f.vertices[2] = ii + n; + f.vertices[3] = i + n; + faces[i] = f; + g.vertices[i] = i; + h.vertices[i] = i + n; + } + faces[n] = g; + faces[n + 1] = h; + return CreateNoCheck(verts, faces); +} + +CPolyhedron::Ptr CPolyhedron::CreateCustomAntiprism( + const vector& bottomBase, const vector& topBase, const double height) +{ + uint32_t n = bottomBase.size(); + if (n < 3) throw std::logic_error("Not enough vertices"); + if (n != topBase.size()) throw std::logic_error("Bases' number of vertices do not match"); + vector verts(n + n); + vector faces(n + n + 2); + TPolyhedronFace f, g, h; + f.vertices.resize(3); + g.vertices.resize(n); + h.vertices.resize(n); + for (uint32_t i = 0; i < n; i++) + { + verts[i] = TPoint3D(bottomBase[i].x, bottomBase[i].y, 0); + verts[n + i] = TPoint3D(topBase[i].x, topBase[i].y, height); + uint32_t ii = (i + 1) % n; + f.vertices[0] = i; + f.vertices[1] = ii; + f.vertices[2] = i + n; + faces[i] = f; + f.vertices[0] = i + n; + f.vertices[1] = ii + n; + f.vertices[2] = ii; + faces[n + i] = f; + g.vertices[i] = i; + h.vertices[i] = n + i; + } + faces[n + n] = g; + faces[n + n + 1] = h; + return CreateNoCheck(verts, faces); +} + +CPolyhedron::Ptr CPolyhedron::CreateParallelepiped( + const TPoint3D& base, const TPoint3D& v1, const TPoint3D& v2, const TPoint3D& v3) +{ + vector verts(8); + vector faces(6); + for (uint32_t i = 0; i < 8; i++) + { + verts[i] = base; + if (i & 1) verts[i] = verts[i] + v1; + if (i & 2) verts[i] = verts[i] + v2; + if (i & 4) verts[i] = verts[i] + v3; + } + TPolyhedronFace f; + f.vertices.resize(4); + f.vertices[0] = 0; + f.vertices[1] = 1; + f.vertices[2] = 3; + f.vertices[3] = 2; + faces[0] = f; + // f.vertices[0]=0; + // f.vertices[1]=1; + f.vertices[2] = 5; + f.vertices[3] = 4; + faces[1] = f; + // f.vertices[0]=0; + f.vertices[1] = 2; + f.vertices[2] = 6; + // f.vertices[3]=4; + faces[2] = f; + for (uint32_t i = 0; i < 3; i++) + { + uint32_t valueAdd = 4 >> i; + faces[i + 3].vertices.resize(4); + for (uint32_t j = 0; j < 4; j++) faces[i + 3].vertices[j] = faces[i].vertices[j] + valueAdd; + } + return CreateNoCheck(verts, faces); +} + +CPolyhedron::Ptr CPolyhedron::CreateBifrustum( + const vector& baseVertices, + double height1, + double ratio1, + double height2, + double ratio2) +{ + // TODO: check special case in which ratio=0. + size_t N = baseVertices.size(); + vector verts(3 * N); + size_t N2 = N + N; + for (size_t i = 0; i < N; i++) + { + double x = baseVertices[i].x; + double y = baseVertices[i].y; + verts[i].x = x; + verts[i].y = y; + verts[i].z = 0; + verts[i + N].x = x * ratio1; + verts[i + N].y = y * ratio1; + verts[i + N].z = height1; + verts[i + N2].x = x * ratio2; + verts[i + N2].y = y * ratio2; + verts[i + N2].z = -height2; // This is not an error. This way, two + // positive heights produce an actual + // bifrustum. + } + vector faces(N2 + 2); + TPolyhedronFace f, g, h; + f.vertices.resize(4); + g.vertices.resize(N); + h.vertices.resize(N); + for (size_t i = 0; i < N; i++) + { + size_t i2 = (i + 1) % N; + f.vertices[0] = i; + f.vertices[1] = i2; + f.vertices[2] = i2 + N; + f.vertices[3] = i + N; + faces[i] = f; + f.vertices[2] = i2 + N2; + f.vertices[3] = i + N2; + faces[i + N] = f; + g.vertices[i] = i + N; + h.vertices[i] = i + N2; + } + faces[N2] = g; + faces[N2 + 1] = h; + return CreateNoCheck(verts, faces); +} + +CPolyhedron::Ptr CPolyhedron::CreateTrapezohedron( + uint32_t numBaseEdges, double baseRadius, double basesDistance) +{ + if (numBaseEdges < 3) throw std::logic_error("Not enough vertices"); + if (basesDistance == 0 || baseRadius == 0) return CreateEmpty(); + size_t numBaseEdges2 = numBaseEdges << 1; + vector verts(numBaseEdges2 + 2); + double space = 2 * M_PI / numBaseEdges; + double shift = space / 2; + double height1 = basesDistance / 2; + double cospii = cos(M_PI / numBaseEdges); + double height2 = height1 * (cospii + 1) / + (1 - cospii); // Apex height, calculated so each face conforms a plane + for (size_t i = 0; i < numBaseEdges; i++) + { + double ang = space * i; + double ang2 = ang + shift; + size_t ii = i + numBaseEdges; + verts[i].x = baseRadius * cos(ang); + verts[i].y = baseRadius * sin(ang); + verts[i].z = -height1; + verts[ii].x = baseRadius * cos(ang2); + verts[ii].y = baseRadius * sin(ang2); + verts[ii].z = height1; + } + verts[numBaseEdges2].x = 0; + verts[numBaseEdges2].y = 0; + verts[numBaseEdges2].z = -height2; + verts[numBaseEdges2 + 1].x = 0; + verts[numBaseEdges2 + 1].y = 0; + verts[numBaseEdges2 + 1].z = height2; + vector faces(numBaseEdges2); + TPolyhedronFace f, g; + f.vertices.resize(4); + g.vertices.resize(4); + f.vertices[3] = numBaseEdges2; + g.vertices[3] = numBaseEdges2 + 1; + for (size_t i = 0; i < numBaseEdges; i++) + { + size_t ii = (i + 1) % numBaseEdges; + size_t i2 = i + numBaseEdges; + f.vertices[0] = i; + f.vertices[1] = i2; + f.vertices[2] = ii; + g.vertices[0] = i2; + g.vertices[1] = ii; + g.vertices[2] = ii + numBaseEdges; + faces[i] = f; + faces[i + numBaseEdges] = g; + } + return CreateNoCheck(verts, faces); +} + +CPolyhedron::Ptr CPolyhedron::CreateJohnsonSolidWithConstantBase( + uint32_t numBaseEdges, double baseRadius, const std::string& components, size_t shifts) +{ + if (baseRadius == 0) return CreateEmpty(); + if (numBaseEdges < 3) throw std::logic_error("Not enough vertices"); + vector parts; + if (!analyzeJohnsonPartsString(components, numBaseEdges, parts)) + throw std::logic_error("Invalid string"); + // Some common values are computed + size_t nParts = parts.size(); + double edgeLength = 2 * baseRadius * sin(M_PI / numBaseEdges); + double antiPrism_height = + sqrt(square(edgeLength) - square(baseRadius) * (2 - 2 * cos(M_PI / numBaseEdges))); + // Vertices' and faces' vectors are computed + size_t nVerts = numBaseEdges * (nParts - 1) + additionalVertices(parts[0], numBaseEdges) + + additionalVertices(*parts.rbegin(), numBaseEdges); + size_t nFaces = 0; + for (size_t i = 0; i < nParts; i++) nFaces += additionalFaces(parts[i], numBaseEdges); + vector verts; + verts.reserve(nVerts); + vector faces; + faces.reserve(nFaces); + // Each base's position is computed. Also, the height is set so that the + // polyhedron is vertically centered in z=0. + double h, m_height = 0; + vector> basePositionInfo(nParts - 1); + for (size_t i = 0; i < nParts - 1; i++) + { + if (parts[i] == PRISM) + h = edgeLength; + else if (parts[i] == ANTIPRISM) + { + h = antiPrism_height; + shifts++; + } + else + h = 0; + basePositionInfo[i] = make_pair(m_height += h, shifts); + } + m_height /= 2; + double semi = M_PI / numBaseEdges; + // All the bases are generated and inserted into the vertices' vector. + for (auto it = basePositionInfo.begin(); it != basePositionInfo.end(); ++it) + generateShiftedBase(numBaseEdges, baseRadius, it->first - m_height, semi * it->second, verts); + size_t initialBase = 0, endBase = 0; + TPolyhedronFace face; + face.vertices.reserve(numBaseEdges); + // Each body is inserted. + for (size_t p = 0; p < nParts; p++) + { + switch (parts[p]) + { + case INF_NO_BODY: + // Inferior base. + face.vertices.resize(numBaseEdges); + for (size_t i = 0; i < numBaseEdges; i++) face.vertices[i] = endBase + i; + faces.push_back(face); + break; + case SUP_NO_BODY: + // Superior base. + face.vertices.resize(numBaseEdges); + for (size_t i = 0; i < numBaseEdges; i++) face.vertices[i] = initialBase + i; + faces.push_back(face); + break; + case UPWARDS_PYRAMID: + { + // Upwards-pointing pyramid. There must be 5 or less vertices. + double apexHeight = baseRadius * sqrt(4 * square(sin(M_PI / numBaseEdges)) - 1); + face.vertices.resize(3); + face.vertices[0] = verts.size(); + face.vertices[1] = initialBase + numBaseEdges - 1; + face.vertices[2] = initialBase; + do + { + faces.push_back(face); + face.vertices[1] = face.vertices[2]; + face.vertices[2]++; + } while (face.vertices[2] < initialBase + numBaseEdges); + verts.emplace_back(0, 0, verts[initialBase].z + apexHeight); + break; + } + case DOWNWARDS_PYRAMID: + { + // Downwards-pointing pyramid. There must be 5 or less vertices. + double apexHeight = baseRadius * sqrt(4 * square(sin(M_PI / numBaseEdges)) - 1); + face.vertices.resize(3); + face.vertices[0] = verts.size(); + face.vertices[1] = endBase + numBaseEdges - 1; + face.vertices[2] = endBase; + do + { + faces.push_back(face); + face.vertices[1] = face.vertices[2]; + face.vertices[2]++; + } while (face.vertices[2] < endBase + numBaseEdges); + verts.emplace_back(0, 0, verts[endBase].z - apexHeight); + break; + } + case UPWARDS_CUPOLA: + // Upwards-pointing cupola. There must be an even amount of + // vertices. + insertCupola( + numBaseEdges, basePositionInfo.rbegin()->second * semi, baseRadius, edgeLength, false, + true, initialBase, verts, faces); + break; + case DOWNWARDS_CUPOLA: + // Downwards-pointing cupola. There must be an even amount of + // vertices. + insertCupola( + numBaseEdges, basePositionInfo[0].second * semi, baseRadius, edgeLength, false, false, + endBase, verts, faces); + break; + case ROTATED_UPWARDS_CUPOLA: + // Upwards-pointing, slightly rotated, cupola. There must be an + // even amount of vertices. + insertCupola( + numBaseEdges, basePositionInfo.rbegin()->second * semi, baseRadius, edgeLength, true, + true, initialBase, verts, faces); + break; + case ROTATED_DOWNWARDS_CUPOLA: + // Downwards-pointing, slightly rotated, cupola. There must be + // an even amount of vertices. + insertCupola( + numBaseEdges, basePositionInfo[0].second * semi, baseRadius, edgeLength, true, false, + endBase, verts, faces); + break; + case PRISM: + // Archimedean prism. + face.vertices.resize(4); + for (size_t i = 0; i < numBaseEdges; i++) + { + size_t ii = (i + 1) % numBaseEdges; + face.vertices[0] = initialBase + i; + face.vertices[1] = endBase + i; + face.vertices[2] = endBase + ii; + face.vertices[3] = initialBase + ii; + faces.push_back(face); + } + break; + case ANTIPRISM: + { + // Archimedean antiprism. + face.vertices.resize(3); + face.vertices[0] = initialBase; + face.vertices[1] = endBase; + face.vertices[2] = initialBase + 1; + bool nextIsEnd = true; + size_t nextEnd = 1; + size_t nextInitial = 2; + for (size_t i = 0; i < numBaseEdges << 1; i++) + { + faces.push_back(face); + face.vertices[0] = face.vertices[1]; + face.vertices[1] = face.vertices[2]; + if (nextIsEnd) + { + face.vertices[2] = endBase + nextEnd; + nextEnd = (nextEnd + 1) % numBaseEdges; + } + else + { + face.vertices[2] = initialBase + nextInitial; + nextInitial = (nextInitial + 1) % numBaseEdges; + } + nextIsEnd = !nextIsEnd; + } + break; + } + case UPWARDS_ROTUNDA: + // Upwards-pointing pentagonal rotunda. Only for bases of + // exactly 10 vertices. + insertRotunda( + basePositionInfo.rbegin()->second * semi, baseRadius, false, true, initialBase, verts, + faces); + break; + case DOWNWARDS_ROTUNDA: + // Downwards-pointing pentagonal rotunda. Only for bases of + // exactly 10 vertices. + insertRotunda( + basePositionInfo[0].second * semi, baseRadius, false, false, endBase, verts, faces); + break; + case ROTATED_UPWARDS_ROTUNDA: + // Upwards-pointing, slightly rotated, pentagonal rotunda. Only + // for bases of exactly 10 vertices. + insertRotunda( + basePositionInfo.rbegin()->second * semi, baseRadius, true, true, initialBase, verts, + faces); + break; + case ROTATED_DOWNWARDS_ROTUNDA: + // Downwards-pointing, slightly rotated, pentagonal rotunda. + // Only for bases of exactly 10 vertices. + insertRotunda( + basePositionInfo[0].second * semi, baseRadius, true, false, endBase, verts, faces); + break; + default: + throw std::logic_error("Internal error"); + } + initialBase = endBase; + endBase += numBaseEdges; + } + return CreateNoCheck(verts, faces); +} + +bool CPolyhedron::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ + if (!polygonsUpToDate) updatePolygons(); + return math::traceRay(tempPolygons, (o - getCPose()).asTPose(), dist); +} + +void CPolyhedron::getEdgesLength(std::vector& lengths) const +{ + lengths.resize(m_Edges.size()); + auto it2 = lengths.begin(); + for (auto it = m_Edges.begin(); it != m_Edges.end(); ++it, ++it2) *it2 = it->length(m_Vertices); +} + +void CPolyhedron::getFacesArea(std::vector& areas) const +{ + areas.resize(m_Faces.size()); + auto it2 = areas.begin(); + for (auto it = m_Faces.begin(); it != m_Faces.end(); ++it, ++it2) *it2 = it->area(m_Vertices); +} + +double CPolyhedron::getVolume() const +{ + // TODO. Calculate as set of pyramids whose apices are situated in the + // center of the polyhedron (will work only with convex polyhedrons). + // Pyramid volume=V=1/3*base area*height. Height=abs((A-V)·N), where A is + // the apex, V is any other vertex, N is the base's normal vector and (·) is + // the scalar product. + TPoint3D center; + getCenter(center); + double res = 0; + if (!polygonsUpToDate) updatePolygons(); + auto itP = tempPolygons.begin(); + vector areas(m_Faces.size()); + getFacesArea(areas); + auto itA = areas.begin(); + for (auto it = m_Faces.begin(); it != m_Faces.end(); ++it, ++itP, ++itA) + res += std::abs(itP->plane.distance(center)) * (*itA); + return res / 3; +} + +void CPolyhedron::getSetOfPolygons(std::vector& vec) const +{ + if (!polygonsUpToDate) updatePolygons(); + size_t N = tempPolygons.size(); + vec.resize(N); + for (size_t i = 0; i < N; i++) vec[i] = tempPolygons[i].poly; +} + +void CPolyhedron::getSetOfPolygonsAbsolute(std::vector& vec) const +{ + vec.resize(m_Faces.size()); + size_t N = m_Vertices.size(); + vector nVerts; + nVerts.resize(N); + CPose3D pose = getCPose(); + for (size_t i = 0; i < N; i++) pose.composePoint(m_Vertices[i], nVerts[i]); + transform( + m_Faces.begin(), m_Faces.end(), vec.begin(), FCreatePolygonFromFace(nVerts)); +} + +void CPolyhedron::makeConvexPolygons() +{ + vector polys, polysTMP, polys2; + getSetOfPolygons(polys); + polys2.reserve(polys.size()); + for (auto it = polys.begin(); it != polys.end(); ++it) + if (mrpt::math::splitInConvexComponents(*it, polysTMP)) + polys2.insert(polys2.end(), polysTMP.begin(), polysTMP.end()); + else + polys2.push_back(*it); + m_Vertices.clear(); + m_Edges.clear(); + m_Faces.clear(); + getVerticesAndFaces(polys2, m_Vertices, m_Faces); + for (auto& mFace : m_Faces) + { + if (!setNormal(mFace, false)) throw std::logic_error("Bad face specification"); + addEdges(mFace); + } +} + +void CPolyhedron::getCenter(TPoint3D& center) const +{ + size_t N = m_Vertices.size(); + if (N == 0) throw new std::logic_error("There are no vertices"); + center.x = center.y = center.z = 0; + for (const auto& mVertice : m_Vertices) + { + center.x += mVertice.x; + center.y += mVertice.y; + center.z += mVertice.z; + } + center.x /= N; + center.y /= N; + center.z /= N; +} + +CPolyhedron::Ptr CPolyhedron::CreateRandomPolyhedron(double radius) +{ + switch (mrpt::random::getRandomGenerator().drawUniform32bit() % 34) + { + case 0: + return CreateTetrahedron(radius); + case 1: + return CreateHexahedron(radius); + case 2: + return CreateOctahedron(radius); + case 3: + return CreateDodecahedron(radius); + case 4: + return CreateIcosahedron(radius); + case 5: + return CreateTruncatedTetrahedron(radius); + case 6: + return CreateTruncatedHexahedron(radius); + case 7: + return CreateTruncatedOctahedron(radius); + case 8: + return CreateTruncatedDodecahedron(radius); + case 9: + return CreateTruncatedIcosahedron(radius); + case 10: + return CreateCuboctahedron(radius); + case 11: + return CreateRhombicuboctahedron(radius); + case 12: + return CreateIcosidodecahedron(radius); + case 13: + return CreateRhombicosidodecahedron(radius); + case 14: + return CreateTriakisTetrahedron(radius); + case 15: + return CreateTriakisOctahedron(radius); + case 16: + return CreateTetrakisHexahedron(radius); + case 17: + return CreateTriakisIcosahedron(radius); + case 18: + return CreatePentakisDodecahedron(radius); + case 19: + return CreateRhombicDodecahedron(radius); + case 20: + return CreateDeltoidalIcositetrahedron(radius); + case 21: + return CreateRhombicTriacontahedron(radius); + case 22: + return CreateDeltoidalHexecontahedron(radius); + case 23: + return CreateArchimedeanRegularPrism( + (mrpt::random::getRandomGenerator().drawUniform32bit() % 10) + 3, radius); + case 24: + return CreateArchimedeanRegularAntiprism( + (mrpt::random::getRandomGenerator().drawUniform32bit() % 10) + 3, radius); + case 25: + return CreateJohnsonSolidWithConstantBase( + ((mrpt::random::getRandomGenerator().drawUniform32bit() % 4) << 1) + 4, radius, "C+"); + case 26: + return CreateJohnsonSolidWithConstantBase( + ((mrpt::random::getRandomGenerator().drawUniform32bit() % 4) << 1) + 4, radius, "C-C+"); + case 27: + return CreateJohnsonSolidWithConstantBase( + ((mrpt::random::getRandomGenerator().drawUniform32bit() % 4) << 1) + 4, radius, "C-PRC+"); + case 28: + return CreateJohnsonSolidWithConstantBase( + ((mrpt::random::getRandomGenerator().drawUniform32bit() % 4) << 1) + 4, radius, "C-AC+"); + case 29: + return CreateJohnsonSolidWithConstantBase(10, radius, "R+"); + case 30: + return CreateJohnsonSolidWithConstantBase(10, radius, "R-PRR+"); + case 31: + return CreateJohnsonSolidWithConstantBase(10, radius, "R-AR+"); + case 32: + return CreateCatalanTrapezohedron( + (mrpt::random::getRandomGenerator().drawUniform32bit() % 5) + 3, radius); + case 33: + return CreateCatalanDoublePyramid( + (mrpt::random::getRandomGenerator().drawUniform32bit() % 5) + 3, radius); + default: + return CreateEmpty(); + } +} + +CPolyhedron::Ptr CPolyhedron::getDual() const +{ + // This methods builds the dual of a given polyhedron, which is assumed to + // be centered in (0,0,0), using polar reciprocation. + // A vertex (x0,y0,z0), which is inside a circunference x^2+y^2+z^2=r^2, its + // dual face will lie on the x0·x+y0·y+z0·z=r^2 plane. + // The new vertices can, then, be calculated as the corresponding + // intersections between three or more planes. + size_t NV = m_Faces.size(); + size_t NE = m_Edges.size(); + size_t NF = m_Vertices.size(); + vector planes(NF); + for (size_t i = 0; i < NF; i++) + { + const TPoint3D& p = m_Vertices[i]; + TPlane& pl = planes[i]; + pl.coefs[0] = p.x; + pl.coefs[1] = p.y; + pl.coefs[2] = p.z; + pl.coefs[3] = -square(p.x) - square(p.y) - square(p.z); + } + CMatrixDynamic incidence(NV, NF); + vector vertices(NV); + for (size_t i = 0; i < NV; i++) + { + for (size_t j = 0; j < NF; j++) incidence(i, j) = false; + vector fPls; + fPls.reserve(m_Faces[i].vertices.size()); + for (auto it = m_Faces[i].vertices.begin(); it != m_Faces[i].vertices.end(); ++it) + { + incidence(i, *it) = true; + fPls.push_back(&planes[*it]); + } + if (!getPlanesIntersection(fPls, vertices[i])) + throw std::logic_error("Dual polyhedron cannot be found"); + } + vector faces(NF); + for (size_t i = 0; i < NF; i++) + for (size_t j = 0; j < NV; j++) + if (incidence(j, i)) faces[i].vertices.push_back(j); + // The following code ensures that the faces' vertex list is in the adequate + // order. + CMatrixDynamic arrayEF(NE, NV); + for (size_t i = 0; i < NE; i++) + for (size_t j = 0; j < NV; j++) arrayEF(i, j) = faceContainsEdge(m_Faces[j], m_Edges[i]); + for (auto& it : faces) + { + vector& face = it.vertices; + if (face.size() <= 3) continue; + size_t index = 0; + while (index < face.size() - 1) + { + bool err = true; + while (err) + { + for (size_t i = 0; i < NE; i++) + if (arrayEF(i, face[index]) && arrayEF(i, face[index + 1])) + { + err = false; + break; + } + if (err) + { + size_t val = face[index + 1]; + face.erase(face.begin() + index + 1); + face.push_back(val); + } + } + index++; + } + } + return std::make_shared(vertices, faces); +} + +CPolyhedron::Ptr CPolyhedron::truncate(double factor) const +{ + if (factor < 0) return CreateEmpty(); + if (factor == 0) + return CreateNoCheck(m_Vertices, m_Faces); + else if (factor < 1) + { + size_t NE = m_Edges.size(); + size_t NV = m_Vertices.size(); + size_t NF = m_Faces.size(); + vector vertices(NE << 1); + vector faces(NV + NF); + for (size_t i = 0; i < NE; i++) + { + const TPoint3D& p1 = m_Vertices[m_Edges[i].v1]; + const TPoint3D& p2 = m_Vertices[m_Edges[i].v2]; + TPoint3D& v1 = vertices[i + i]; + TPoint3D& v2 = vertices[i + i + 1]; + for (size_t j = 0; j < 3; j++) + { + double d = (p2[j] - p1[j]) * factor / 2; + v1[j] = p1[j] + d; + v2[j] = p2[j] - d; + } + faces[m_Edges[i].v1].vertices.push_back(i + i); + faces[m_Edges[i].v2].vertices.push_back(i + i + 1); + } + for (size_t i = 0; i < NV; i++) + { + vector& f = faces[i].vertices; + size_t sf = f.size(); + if (sf == 3) continue; + for (size_t j = 1; j < sf - 1; j++) + { + const TPolyhedronEdge& e1 = m_Edges[f[j - 1] / 2]; + for (;;) + { + const TPolyhedronEdge& e2 = m_Edges[f[j] / 2]; + if (!((e1.v1 == i || e1.v2 == i) && (e2.v1 == i || e2.v2 == i))) + THROW_EXCEPTION("En algo te has equivocado, chaval."); + if (searchForFace(m_Faces, i, (e1.v1 == i) ? e1.v2 : e1.v1, (e2.v1 == i) ? e2.v2 : e2.v1)) + break; + uint32_t tmpV = f[j]; + f.erase(f.begin() + j); + f.push_back(tmpV); + } + } + } + for (size_t i = 0; i < NF; i++) + { + vector& f = faces[i + NV].vertices; + const vector& cf = m_Faces[i].vertices; + size_t hmV = cf.size(); + f.reserve(hmV << 1); + for (size_t j = 0; j < hmV; j++) + { + size_t where; + if (searchForEdge(m_Edges, cf[j], cf[(j + 1) % hmV], where)) + { + f.push_back(where << 1); + f.push_back((where << 1) + 1); + } + else + { + f.push_back((where << 1) + 1); + f.push_back(where << 1); + } + } + } + return std::make_shared(vertices, faces); + } + else if (factor == 1) + { + size_t NE = m_Edges.size(); + size_t NV = m_Vertices.size(); + size_t NF = m_Faces.size(); + vector vertices(NE); + vector faces(NV + NF); + for (size_t i = 0; i < NE; i++) + { + const TPoint3D& p1 = m_Vertices[m_Edges[i].v1]; + const TPoint3D& p2 = m_Vertices[m_Edges[i].v2]; + TPoint3D& dst = vertices[i]; + for (size_t j = 0; j < 3; j++) dst[j] = (p1[j] + p2[j]) / 2; + faces[m_Edges[i].v1].vertices.push_back(i); + faces[m_Edges[i].v2].vertices.push_back(i); + } + for (size_t i = 0; i < NV; i++) + { + vector& f = faces[i].vertices; + size_t sf = f.size(); + if (sf == 3) continue; + for (size_t j = 1; j < sf - 1; j++) + { + const TPolyhedronEdge& e1 = m_Edges[f[j - 1]]; + for (;;) + { + const TPolyhedronEdge& e2 = m_Edges[f[j - 1]]; + if (!((e1.v1 == i || e1.v2 == i) && (e2.v1 == 1 || e2.v2 == i))) + THROW_EXCEPTION("En algo te has equivocado, chaval."); + if (searchForFace(m_Faces, i, (e1.v1 == i) ? e1.v2 : e1.v1, (e2.v1 == i) ? e2.v2 : e2.v1)) + break; + uint32_t tmpV = f[j]; + f.erase(f.begin() + j); + f.push_back(tmpV); + } + } + } + for (size_t i = 0; i < NF; i++) + { + vector& f = faces[i + NV].vertices; + const vector& cf = m_Faces[i].vertices; + size_t hmV = cf.size(); + f.reserve(hmV); + for (size_t j = 0; j < hmV; j++) + { + size_t where; + searchForEdge(m_Edges, cf[j], cf[(j + 1) % hmV], where); + f.push_back(where); + } + } + return std::make_shared(vertices, faces); + } + else + return CreateEmpty(); +} + +CPolyhedron::Ptr CPolyhedron::cantellate(double factor) const +{ + if (factor < 0) + return CreateEmpty(); + else if (factor == 0) + return CreateNoCheck(m_Vertices, m_Faces); + size_t NV = m_Vertices.size(); + size_t NE = m_Edges.size(); + size_t NF = m_Faces.size(); + vector origFaces(NF); + getSetOfPolygons(origFaces); + TPoint3D cnt; + getCenter(cnt); + vector polyCenters(NF); + vector polyNewCenters(NF); + size_t NNV = 0; + for (size_t i = 0; i < NF; i++) + { + origFaces[i].getCenter(polyCenters[i]); + polyCenters[i] -= cnt; + polyNewCenters[i] = polyCenters[i]; + polyNewCenters[i] *= (1 + factor); + polyNewCenters[i] += cnt; + NNV += origFaces[i].size(); + } + vector vertices(NNV); + vector faces(NF + NV + NE); + size_t ind = 0; + for (size_t i = 0; i < NF; i++) + { + const TPoint3D& oC = polyCenters[i]; + const TPoint3D& nC = polyNewCenters[i]; + const TPolygon3D& oP = origFaces[i]; + vector& f = faces[i].vertices; + size_t oPS = oP.size(); + for (size_t j = 0; j < oPS; j++) + { + vertices[j + ind] = nC + (oP[j] - oC); + f.push_back(j + ind); + size_t curr = m_Faces[i].vertices[j]; + faces[NF + curr].vertices.push_back(j + ind); + size_t edge; + searchForEdge(m_Edges, curr, m_Faces[i].vertices[(j + oPS - 1) % oPS], edge); + faces[NF + NV + edge].vertices.push_back(j + ind); + searchForEdge(m_Edges, curr, m_Faces[i].vertices[(j + 1) % oPS], edge); + faces[NF + NV + edge].vertices.push_back(j + ind); + } + ind += oPS; + } + auto begin = faces.begin(), edgeBegin = faces.begin() + NF + NV, end = faces.end(); + for (auto it = faces.begin() + NF; it != faces.begin() + NF + NV; ++it) + { + vector& f = it->vertices; + if (f.size() == 3) continue; + for (size_t i = 1; i < f.size() - 1; i++) + for (;;) + if (searchForEdge(edgeBegin, end, f[i - 1], f[i])) + break; + else + { + uint32_t tmp = f[i]; + f.erase(f.begin() + i); + f.push_back(tmp); + } + } + for (auto it = faces.begin() + NF + NV; it != faces.end(); ++it) + { + vector& f = it->vertices; // Will always have exactly 4 vertices + for (size_t i = 1; i < 3; i++) + for (;;) + if (searchForEdge(begin, edgeBegin, f[i - 1], f[i])) + break; + else + { + uint32_t tmp = f[i]; + f.erase(f.begin() + i); + f.push_back(tmp); + } + } + return std::make_shared(vertices, faces); +} + +CPolyhedron::Ptr CPolyhedron::augment(double height) const +{ + size_t NV = m_Vertices.size(); + size_t NF = m_Faces.size(); + vector vertices(NV + NF); + std::copy(m_Vertices.begin(), m_Vertices.end(), vertices.begin()); + size_t tnf = 0; + for (const auto& mFace : m_Faces) tnf += mFace.vertices.size(); + vector faces(tnf); + TPolygon3D tmp; + TPlane pTmp; + TPoint3D cTmp; + size_t iF = 0; + TPoint3D phCenter; + getCenter(phCenter); + TPolyhedronFace fTmp; + fTmp.vertices.resize(3); + for (size_t i = 0; i < NF; i++) + { + TPoint3D& vertex = vertices[NV + i]; + const vector& face = m_Faces[i].vertices; + size_t N = face.size(); + tmp.resize(N); + for (size_t j = 0; j < N; j++) tmp[j] = m_Vertices[face[j]]; + tmp.getBestFittingPlane(pTmp); + pTmp.unitarize(); + tmp.getCenter(cTmp); + if (pTmp.evaluatePoint(phCenter) > 0) + for (size_t j = 0; j < 3; j++) vertex[j] = cTmp[j] - height * pTmp.coefs[j]; + else + for (size_t j = 0; j < 3; j++) vertex[j] = cTmp[j] + height * pTmp.coefs[j]; + fTmp.vertices[0] = NV + i; + for (size_t j = 0; j < N; j++) + { + fTmp.vertices[1] = face[j]; + fTmp.vertices[2] = face[(j + 1) % N]; + faces[iF + j] = fTmp; + } + iF += N; + } + return CreateNoCheck(vertices, faces); +} + +CPolyhedron::Ptr CPolyhedron::augment(double height, size_t numVertices) const +{ + size_t NV = m_Vertices.size(); + size_t NF = m_Faces.size(); + size_t tnf = 0; + size_t tnv = NV; + for (const auto& mFace : m_Faces) + if (mFace.vertices.size() == numVertices) + { + tnf += numVertices; + tnv++; + } + else + tnf++; + if (tnv == NV) return CreateNoCheck(m_Vertices, m_Faces); + vector vertices(tnv); + std::copy(m_Vertices.begin(), m_Vertices.end(), vertices.begin()); + vector faces(tnf); + TPolygon3D tmp(numVertices); + TPlane pTmp; + TPoint3D cTmp; + size_t iF = 0; + size_t iV = NV; + TPoint3D phCenter; + getCenter(phCenter); + TPolyhedronFace fTmp; + fTmp.vertices.resize(3); + for (size_t i = 0; i < NF; i++) + { + const vector& face = m_Faces[i].vertices; + size_t N = face.size(); + if (N != numVertices) + { + faces[iF].vertices = face; + iF++; + continue; + } + TPoint3D& vertex = vertices[iV]; + for (size_t j = 0; j < numVertices; j++) tmp[j] = m_Vertices[face[j]]; + tmp.getBestFittingPlane(pTmp); + pTmp.unitarize(); + tmp.getCenter(cTmp); + if (pTmp.evaluatePoint(phCenter) > 0) + for (size_t j = 0; j < 3; j++) vertex[j] = cTmp[j] - height * pTmp.coefs[j]; + else + for (size_t j = 0; j < 3; j++) vertex[j] = cTmp[j] + height * pTmp.coefs[j]; + fTmp.vertices[0] = iV; + for (size_t j = 0; j < N; j++) + { + fTmp.vertices[1] = face[j]; + fTmp.vertices[2] = face[(j + 1) % N]; + faces[iF + j] = fTmp; + } + iF += N; + iV++; + } + return CreateNoCheck(vertices, faces); +} + +CPolyhedron::Ptr CPolyhedron::augment(bool direction) const +{ + size_t NV = m_Vertices.size(); + size_t NF = m_Faces.size(); + vector vertices(NV + NF); + std::copy(m_Vertices.begin(), m_Vertices.end(), vertices.begin()); + size_t tnf = 0; + for (const auto& mFace : m_Faces) tnf += mFace.vertices.size(); + vector faces(tnf); + TPolygon3D tmp; + TPlane pTmp; + TPoint3D cTmp; + size_t iF = 0; + TPoint3D phCenter; + getCenter(phCenter); + TPolyhedronFace fTmp; + fTmp.vertices.resize(3); + for (size_t i = 0; i < NF; i++) + { + TPoint3D& vertex = vertices[NV + i]; + const vector& face = m_Faces[i].vertices; + size_t N = face.size(); + tmp.resize(N); + for (size_t j = 0; j < N; j++) tmp[j] = m_Vertices[face[j]]; + tmp.getCenter(cTmp); + double height = getHeight(tmp, cTmp); // throws std::logic_error + tmp.getBestFittingPlane(pTmp); + pTmp.unitarize(); + if ((pTmp.evaluatePoint(phCenter) < 0) == direction) + for (size_t j = 0; j < 3; j++) vertex[j] = cTmp[j] - height * pTmp.coefs[j]; + else + for (size_t j = 0; j < 3; j++) vertex[j] = cTmp[j] + height * pTmp.coefs[j]; + fTmp.vertices[0] = NV + i; + for (size_t j = 0; j < N; j++) + { + fTmp.vertices[1] = face[j]; + fTmp.vertices[2] = face[(j + 1) % N]; + faces[iF + j] = fTmp; + } + iF += N; + } + return CreateNoCheck(vertices, faces); +} + +CPolyhedron::Ptr CPolyhedron::augment(size_t numVertices, bool direction) const +{ + size_t NV = m_Vertices.size(); + size_t NF = m_Faces.size(); + size_t tnf = 0; + size_t tnv = NV; + for (const auto& mFace : m_Faces) + if (mFace.vertices.size() == numVertices) + { + tnf += numVertices; + tnv++; + } + else + tnf++; + if (tnv == NV) return CreateNoCheck(m_Vertices, m_Faces); + vector vertices(tnv); + std::copy(m_Vertices.begin(), m_Vertices.end(), vertices.begin()); + vector faces(tnf); + TPolygon3D tmp(numVertices); + TPlane pTmp; + TPoint3D cTmp; + size_t iF = 0; + size_t iV = NV; + TPoint3D phCenter; + getCenter(phCenter); + TPolyhedronFace fTmp; + fTmp.vertices.resize(3); + for (size_t i = 0; i < NF; i++) + { + const vector& face = m_Faces[i].vertices; + size_t N = face.size(); + if (N != numVertices) + { + faces[iF].vertices = face; + iF++; + continue; + } + TPoint3D& vertex = vertices[iV]; + for (size_t j = 0; j < numVertices; j++) tmp[j] = m_Vertices[face[j]]; + tmp.getBestFittingPlane(pTmp); + pTmp.unitarize(); + tmp.getCenter(cTmp); + double height = getHeight(tmp, cTmp); // throws std::logic_error + if ((pTmp.evaluatePoint(phCenter) < 0) == direction) + for (size_t j = 0; j < 3; j++) vertex[j] = cTmp[j] - height * pTmp.coefs[j]; + else + for (size_t j = 0; j < 3; j++) vertex[j] = cTmp[j] + height * pTmp.coefs[j]; + fTmp.vertices[0] = iV; + for (size_t j = 0; j < N; j++) + { + fTmp.vertices[1] = face[j]; + fTmp.vertices[2] = face[(j + 1) % N]; + faces[iF + j] = fTmp; + } + iF += N; + iV++; + } + return CreateNoCheck(vertices, faces); +} + +CPolyhedron::Ptr CPolyhedron::rotate(double angle) const +{ + vector vertices(m_Vertices); + double c = cos(angle), s = sin(angle); + for (auto& vertice : vertices) + { + double A = vertice.x; + double B = vertice.y; + vertice.x = A * c - B * s; + vertice.y = B * c + A * s; + } + return CreateNoCheck(vertices, m_Faces); +} + +CPolyhedron::Ptr CPolyhedron::scale(double factor) const +{ + vector vertices(m_Vertices); + if (factor <= 0) throw std::logic_error("Factor must be a strictly positive number"); + for (auto& vertice : vertices) + { + vertice.x *= factor; + vertice.y *= factor; + } + return CreateNoCheck(vertices, m_Faces); +} + +vector CPolyhedron::generateBase(uint32_t numBaseEdges, double baseRadius) +{ + vector base(numBaseEdges); + for (size_t i = 0; i < numBaseEdges; i++) + { + double ang = 2 * M_PI * i / numBaseEdges; + base[i].x = baseRadius * cos(ang); + base[i].y = baseRadius * sin(ang); + } + return base; +} + +vector CPolyhedron::generateShiftedBase(uint32_t numBaseEdges, double baseRadius) +{ + vector base(numBaseEdges); + double shift = M_PI / numBaseEdges; + for (size_t i = 0; i < numBaseEdges; i++) + { + double ang = shift + 2 * M_PI * i / numBaseEdges; + base[i].x = baseRadius * cos(ang); + base[i].y = baseRadius * sin(ang); + } + return base; +} + +void CPolyhedron::generateBase( + uint32_t numBaseEdges, double baseRadius, double height, vector& vec) +{ + for (size_t i = 0; i < numBaseEdges; i++) + { + double ang = 2 * M_PI * i / numBaseEdges; + vec.emplace_back(baseRadius * cos(ang), baseRadius * sin(ang), height); + } +} + +void CPolyhedron::generateShiftedBase( + uint32_t numBaseEdges, double baseRadius, double height, double shift, vector& vec) +{ + for (size_t i = 0; i < numBaseEdges; i++) + { + double ang = 2 * M_PI * i / numBaseEdges + shift; + vec.emplace_back(baseRadius * cos(ang), baseRadius * sin(ang), height); + } +} + +void CPolyhedron::updatePolygons() const +{ + tempPolygons.resize(m_Faces.size()); + transform( + m_Faces.begin(), m_Faces.end(), tempPolygons.begin(), + FCreatePolygonFromFace(m_Vertices)); + polygonsUpToDate = true; +} + +bool CPolyhedron::setNormal(TPolyhedronFace& f, bool doCheck) +{ + size_t N = doCheck ? f.vertices.size() : 3; + TPolygon3D poly(N); + for (size_t i = 0; i < N; i++) poly[i] = m_Vertices[f.vertices[i]]; + TPlane tmp; + if (!poly.getPlane(tmp)) return false; + f.normal = tmp.getNormalVector(); + TPoint3D c; + getCenter(c); + if (tmp.evaluatePoint(c) > 0) f.normal *= -1; + return true; +} + +void CPolyhedron::addEdges(const TPolyhedronFace& f) +{ + TPolyhedronEdge e; + auto it = f.vertices.begin(); + e.v1 = *it; + ++it; + while (it != f.vertices.end()) + { + e.v2 = *it; + if (find(m_Edges.begin(), m_Edges.end(), e) == m_Edges.end()) m_Edges.push_back(e); + e.v1 = e.v2; + ++it; + } + e.v2 = *(f.vertices.begin()); + if (find(m_Edges.begin(), m_Edges.end(), e) == m_Edges.end()) m_Edges.push_back(e); +} + +bool CPolyhedron::checkConsistence( + const vector& vertices, const vector& faces) +{ + size_t N = vertices.size(); + if (vertices.size() > 0) + for (auto it = vertices.begin(); it != vertices.end() - 1; ++it) + for (auto it2 = it + 1; it2 != vertices.end(); ++it2) + if (*it == *it2) return false; + for (const auto& face : faces) + { + const vector& e = face.vertices; + for (unsigned int it2 : e) + if (it2 >= N) return false; + } + return true; +} + +size_t CPolyhedron::edgesInVertex(size_t vertex) const +{ + size_t res = 0; + for (const auto& mEdge : m_Edges) + if (mEdge.v1 == vertex || mEdge.v2 == vertex) res++; + return res; +} + +size_t CPolyhedron::facesInVertex(size_t vertex) const +{ + size_t res = 0; + for (const auto& mFace : m_Faces) + if (find(mFace.vertices.begin(), mFace.vertices.end(), vertex) != mFace.vertices.end()) res++; + return res; +} + +CArchive& mrpt::viz::operator>>(CArchive& in, CPolyhedron::TPolyhedronEdge& o) +{ + in >> o.v1 >> o.v2; + return in; +} + +CArchive& mrpt::viz::operator<<(CArchive& out, const CPolyhedron::TPolyhedronEdge& o) +{ + out << o.v1 << o.v2; + return out; +} + +CArchive& mrpt::viz::operator>>(CArchive& in, CPolyhedron::TPolyhedronFace& o) +{ + in >> o.vertices >> o.normal[0] >> o.normal[1] >> o.normal[2]; + return in; +} + +CArchive& mrpt::viz::operator<<(CArchive& out, const CPolyhedron::TPolyhedronFace& o) +{ + out << o.vertices << o.normal[0] << o.normal[1] << o.normal[2]; + return out; +} + +uint8_t CPolyhedron::serializeGetVersion() const { return 2; } +void CPolyhedron::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + // version 0 + out << m_Vertices << m_Faces << m_Wireframe; + VisualObjectParams_Triangles::params_serialize(out); // v1 + VisualObjectParams_Lines::params_serialize(out); // v2 +} + +void CPolyhedron::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + THROW_EXCEPTION("Old version deserialization not supported"); + break; + + case 2: + readFromStreamRender(in); + in >> m_Vertices >> m_Faces >> m_Wireframe; + VisualObjectParams_Triangles::params_deserialize(in); + VisualObjectParams_Lines::params_deserialize(in); + + if (!checkConsistence(m_Vertices, m_Faces)) + throw std::logic_error("Inconsistent data read from stream"); + for (auto& mFace : m_Faces) + { + if (!setNormal(mFace)) throw std::logic_error("Bad face specification"); + addEdges(mFace); + } + + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +auto CPolyhedron::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + auto bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); + for (const auto& pt : m_Vertices) bb.updateWithPoint(pt); + return bb; +} + +/*CPolyhedron::Ptr CPolyhedron::CreateCuboctahedron(double radius) { + if (radius==0) return CreateEmpty(); + vector verts; + vector faces; + double d=radius/sqrt(2.0); + verts.push_back(TPoint3D(d,0,d)); + verts.push_back(TPoint3D(0,d,d)); + verts.push_back(TPoint3D(0,-d,d)); + verts.push_back(TPoint3D(-d,0,d)); + verts.push_back(TPoint3D(d,d,0)); + verts.push_back(TPoint3D(d,-d,0)); + verts.push_back(TPoint3D(-d,d,0)); + verts.push_back(TPoint3D(-d,-d,0)); + verts.push_back(TPoint3D(d,0,-d)); + verts.push_back(TPoint3D(0,d,-d)); + verts.push_back(TPoint3D(0,-d,-d)); + verts.push_back(TPoint3D(-d,0,-d)); + TPolyhedronFace f; + static uint32_t faces3[]={0,1,4, 0,2,5, 1,3,6, 2,3,7, 8,9,4, 8,10,5, 9,11,6, +10,11,7}; + static uint32_t faces4[]={0,1,3,2, 8,9,11,10, 0,4,8,5, 1,4,9,6, 2,5,10,7, +3,6,11,7}; + for (uint32_t *p=reinterpret_cast(&faces3);p<24+reinterpret_cast(&faces3);p+=3) { + f.vertices.insert(f.vertices.begin(),p,p+3); + faces.push_back(f); + f.vertices.clear(); + } + for (uint32_t *p=reinterpret_cast(&faces4);p<24+reinterpret_cast(&faces4);p+=4) { + f.vertices.insert(f.vertices.begin(),p,p+4); + faces.push_back(f); + f.vertices.clear(); + } + return CreateNoCheck(verts,faces); +}*/ + +CPolyhedron::Ptr CPolyhedron::CreateTetrahedron(double radius) +{ + CPolyhedron::Ptr tetra = CreateJohnsonSolidWithConstantBase(3, radius * sqrt(8.0) / 3.0, "P+"); + for (auto& mVertice : tetra->m_Vertices) mVertice.z -= radius / 3; + return tetra; +} +CPolyhedron::Ptr CPolyhedron::CreateHexahedron(double radius) +{ + if (radius == 0.0) return CreateEmpty(); + double r = radius / sqrt(3.0); + return CreateCubicPrism(-r, r, -r, r, -r, r); +} +CPolyhedron::Ptr CPolyhedron::CreateOctahedron(double radius) +{ + return CreateJohnsonSolidWithConstantBase(4, radius, "P-P+"); +} +CPolyhedron::Ptr CPolyhedron::CreateDodecahedron(double radius) +{ + return CreateIcosahedron(radius / sqrt(15 - 6 * sqrt(5.0)))->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreateIcosahedron(double radius) +{ + double ang = M_PI / 5; + double s2 = 4 * square(sin(ang)); + double prop = sqrt(s2 - 1) + sqrt(s2 - 2 + 2 * cos(ang)) / 2; + return CreateJohnsonSolidWithConstantBase(5, radius / prop, "P-AP+", 1); +} +CPolyhedron::Ptr CPolyhedron::CreateTruncatedTetrahedron(double radius) +{ + return CreateTetrahedron(radius * sqrt(27.0 / 11.0))->truncate(2.0 / 3.0); +} +CPolyhedron::Ptr CPolyhedron::CreateCuboctahedron(double radius) +{ + return CreateHexahedron(radius * sqrt(1.5))->truncate(1.0); +} +CPolyhedron::Ptr CPolyhedron::CreateTruncatedHexahedron(double radius) +{ + return CreateHexahedron(radius * sqrt(3.0 / (5 - sqrt(8.0))))->truncate(2 - sqrt(2.0)); +} +CPolyhedron::Ptr CPolyhedron::CreateTruncatedOctahedron(double radius) +{ + return CreateOctahedron(radius * 3 / sqrt(5.0))->truncate(2.0 / 3.0); +} +CPolyhedron::Ptr CPolyhedron::CreateRhombicuboctahedron(double radius, bool type) +{ + return CreateJohnsonSolidWithConstantBase( + 8, radius / sqrt(1 + square(sin(M_PI / 8))), type ? "C-PRC+" : "GC-PRC+", 3); +} +CPolyhedron::Ptr CPolyhedron::CreateIcosidodecahedron(double radius, bool type) +{ + return CreateJohnsonSolidWithConstantBase(10, radius, type ? "GR-R+" : "R-R+", 1); +} +CPolyhedron::Ptr CPolyhedron::CreateTruncatedDodecahedron(double radius) +{ + return CreateDodecahedron(radius * sqrt(45.0) / sqrt(27 + 6 * sqrt(5.0))) + ->truncate(1 - sqrt(0.2)); +} +CPolyhedron::Ptr CPolyhedron::CreateTruncatedIcosahedron(double radius) +{ + return CreateIcosahedron(radius * sqrt(45.0) / sqrt(25 + 4 * sqrt(5.0)))->truncate(2.0 / 3.0); +} +CPolyhedron::Ptr CPolyhedron::CreateRhombicosidodecahedron(double radius) +{ + return CreateIcosahedron(radius * sqrt(10.0 / (35.0 + 9.0 * sqrt(5.0)))) + ->cantellate(1.5 * (sqrt(5.0) - 1)); +} +CPolyhedron::Ptr CPolyhedron::CreatePentagonalRotunda(double radius) +{ + return CreateJohnsonSolidWithConstantBase(10, radius, "R+"); +} +CPolyhedron::Ptr CPolyhedron::CreateTriakisTetrahedron(double radius) +{ + return CreateTruncatedTetrahedron(radius * 3 / sqrt(33.0))->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreateRhombicDodecahedron(double radius) +{ + return CreateCuboctahedron(radius / sqrt(2.0))->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreateTriakisOctahedron(double radius) +{ + return CreateTruncatedHexahedron(radius / sqrt((5 - sqrt(8.0))))->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreateTetrakisHexahedron(double radius) +{ + return CreateTruncatedOctahedron(radius * sqrt(0.6))->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreateDeltoidalIcositetrahedron(double radius) +{ + return CreateRhombicuboctahedron(radius / sqrt(7 - sqrt(32.0)), true)->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreateRhombicTriacontahedron(double radius) +{ + return CreateIcosidodecahedron(radius * sqrt(2 / (5 - sqrt(5.0))), true)->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreateTriakisIcosahedron(double radius) +{ + return CreateTruncatedDodecahedron(radius * sqrt(5 / (25 - 8 * sqrt(5.0))))->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreatePentakisDodecahedron(double radius) +{ + return CreateTruncatedIcosahedron(radius * sqrt(3 / (17 - 6 * sqrt(5.0))))->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreateDeltoidalHexecontahedron(double radius) +{ + return CreateRhombicosidodecahedron(radius * 3.0 / sqrt(15 - 2 * sqrt(5.0)))->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreateCubicPrism(const TPoint3D& p1, const TPoint3D& p2) +{ + return CreateCubicPrism(p1.x, p2.x, p1.y, p2.y, p1.z, p2.z); +} +CPolyhedron::Ptr CPolyhedron::CreateFrustum( + const vector& baseVertices, double height, double ratio) +{ + return CreateTruncatedPyramid(baseVertices, height, ratio); +} +CPolyhedron::Ptr CPolyhedron::CreateCustomPrism(const vector& baseVertices, double height) +{ + return CreateTruncatedPyramid(baseVertices, height, 1.0); +} +CPolyhedron::Ptr CPolyhedron::CreateRegularAntiprism( + uint32_t numBaseEdges, double baseRadius, double height) +{ + return CreateCustomAntiprism( + generateBase(numBaseEdges, baseRadius), generateShiftedBase(numBaseEdges, baseRadius), + height); +} +CPolyhedron::Ptr CPolyhedron::CreateRegularPrism( + uint32_t numBaseEdges, double baseRadius, double height) +{ + return CreateCustomPrism(generateBase(numBaseEdges, baseRadius), height); +} +CPolyhedron::Ptr CPolyhedron::CreateRegularPyramid( + uint32_t numBaseEdges, double baseRadius, double height) +{ + return CreatePyramid(generateBase(numBaseEdges, baseRadius), height); +} +CPolyhedron::Ptr CPolyhedron::CreateRegularDoublePyramid( + uint32_t numBaseEdges, double baseRadius, double height1, double height2) +{ + return CreateDoublePyramid(generateBase(numBaseEdges, baseRadius), height1, height2); +} +CPolyhedron::Ptr CPolyhedron::CreateArchimedeanRegularPrism( + uint32_t numBaseEdges, double baseRadius) +{ + return CreateJohnsonSolidWithConstantBase(numBaseEdges, baseRadius, "PR"); +} +CPolyhedron::Ptr CPolyhedron::CreateArchimedeanRegularAntiprism( + uint32_t numBaseEdges, double baseRadius) +{ + return CreateJohnsonSolidWithConstantBase(numBaseEdges, baseRadius, "A"); +} +CPolyhedron::Ptr CPolyhedron::CreateRegularTruncatedPyramid( + uint32_t numBaseEdges, double baseRadius, double height, double ratio) +{ + return CreateTruncatedPyramid(generateBase(numBaseEdges, baseRadius), height, ratio); +} +CPolyhedron::Ptr CPolyhedron::CreateRegularFrustum( + uint32_t numBaseEdges, double baseRadius, double height, double ratio) +{ + return CreateRegularTruncatedPyramid(numBaseEdges, baseRadius, height, ratio); +} +CPolyhedron::Ptr CPolyhedron::CreateRegularBifrustum( + uint32_t numBaseEdges, + double baseRadius, + double height1, + double ratio1, + double height2, + double ratio2) +{ + return CreateBifrustum(generateBase(numBaseEdges, baseRadius), height1, ratio1, height2, ratio2); +} +CPolyhedron::Ptr CPolyhedron::CreateCupola(uint32_t numBaseEdges, double edgeLength) +{ + return CreateJohnsonSolidWithConstantBase( + numBaseEdges, edgeLength / (2 * sin(M_PI / numBaseEdges)), "C+"); +} +CPolyhedron::Ptr CPolyhedron::CreateCatalanTrapezohedron(uint32_t numBaseEdges, double height) +{ + return CreateArchimedeanRegularAntiprism(numBaseEdges, height)->getDual(); +} +CPolyhedron::Ptr CPolyhedron::CreateCatalanDoublePyramid(uint32_t numBaseEdges, double height) +{ + return CreateArchimedeanRegularPrism(numBaseEdges, height)->getDual(); +} + +CPolyhedron::Ptr CPolyhedron::CreateNoCheck( + const vector& vertices, const vector& faces) +{ + return CPolyhedron::Create(vertices, faces, false); +} +CPolyhedron::Ptr CPolyhedron::CreateEmpty() { return CPolyhedron::Create(); } diff --git a/libs/viz/src/CSetOfLines.cpp b/libs/viz/src/CSetOfLines.cpp new file mode 100644 index 0000000000..7e55505334 --- /dev/null +++ b/libs/viz/src/CSetOfLines.cpp @@ -0,0 +1,143 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CSetOfLines, CVisualObject, mrpt::viz) + +/** Constructor */ +CSetOfLines::CSetOfLines() +{ + // Override default pointsize=1 in points shader: + VisualObjectParams_Points::setPointSize(0); +} + +/** Constructor with a initial set of lines. */ +CSetOfLines::CSetOfLines(const std::vector& sgms, bool antiAliasing) : m_Segments(sgms) +{ + // Override default pointsize=1 in points shader: + VisualObjectParams_Points::setPointSize(0); + + VisualObjectParams_Lines::setLineWidth(1); + VisualObjectParams_Lines::enableAntiAliasing(antiAliasing); +} + +/*--------------------------------------------------------------- + setLineByIndex + ---------------------------------------------------------------*/ +void CSetOfLines::setLineByIndex(size_t index, const mrpt::math::TSegment3D& segm) +{ + MRPT_START + if (index >= m_Segments.size()) THROW_EXCEPTION("Index out of bounds"); + CVisualObject::notifyChange(); + m_Segments[index] = segm; + MRPT_END +} + +uint8_t CSetOfLines::serializeGetVersion() const { return 5; } +void CSetOfLines::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_Segments; + VisualObjectParams_Lines::params_serialize(out); // v5 + VisualObjectParams_Points::params_serialize(out); // v4 +} + +void CSetOfLines::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + { + readFromStreamRender(in); + CVectorFloat x0, y0, z0, x1, y1, z1; + in >> x0 >> y0 >> z0 >> x1 >> y1 >> z1; + if (version >= 1) VisualObjectParams_Lines::setLineWidth(in.ReadAs()); + size_t N = x0.size(); + m_Segments.resize(N); + for (size_t i = 0; i < N; i++) + { + m_Segments[i][0][0] = x0[i]; + m_Segments[i][0][1] = y0[i]; + m_Segments[i][0][2] = z0[i]; + m_Segments[i][1][0] = x1[i]; + m_Segments[i][1][1] = y1[i]; + m_Segments[i][1][2] = z1[i]; + } + } + break; + case 2: + case 3: + case 4: + { + readFromStreamRender(in); + in >> m_Segments; + VisualObjectParams_Lines::setLineWidth(in.ReadAs()); + if (version >= 3) VisualObjectParams_Lines::enableAntiAliasing(in.ReadAs()); + + if (version >= 4) + VisualObjectParams_Points::params_deserialize(in); + else + VisualObjectParams_Points::setPointSize(0); + } + break; + case 5: + { + readFromStreamRender(in); + in >> m_Segments; + VisualObjectParams_Lines::params_deserialize(in); // v5 + VisualObjectParams_Points::params_deserialize(in); // v4 + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +auto CSetOfLines::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + auto bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); + + for (const auto& s : m_Segments) + { + for (size_t p = 0; p < 2; p++) + { + const TPoint3D& pt = s[p]; + bb.updateWithPoint(pt); + } + } + + return bb; +} + +void CSetOfLines::getLineByIndex( + size_t index, double& x0, double& y0, double& z0, double& x1, double& y1, double& z1) const +{ + ASSERT_(index < m_Segments.size()); + const mrpt::math::TPoint3D& p0 = m_Segments[index].point1; + const mrpt::math::TPoint3D& p1 = m_Segments[index].point2; + x0 = p0.x; + y0 = p0.y; + z0 = p0.z; + x1 = p1.x; + y1 = p1.y; + z1 = p1.z; +} diff --git a/libs/viz/src/CSetOfObjects.cpp b/libs/viz/src/CSetOfObjects.cpp new file mode 100644 index 0000000000..35cad435ed --- /dev/null +++ b/libs/viz/src/CSetOfObjects.cpp @@ -0,0 +1,234 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include + +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::poses; +using namespace mrpt::math; +using namespace std; + +#include +using namespace mrpt::serialization::metaprogramming; + +IMPLEMENTS_SERIALIZABLE(CSetOfObjects, CVisualObject, mrpt::viz) + +void CSetOfObjects::clear() { m_objects.clear(); } + +uint8_t CSetOfObjects::serializeGetVersion() const { return 0; } +void CSetOfObjects::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + + out.WriteAs(m_objects.size()); + for (const auto& m_object : m_objects) out << *m_object; +} + +/*--------------------------------------------------------------- + Implements the reading from a CStream capability of + CSerializable objects + ---------------------------------------------------------------*/ +void CSetOfObjects::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + readFromStreamRender(in); + + uint32_t n; + in >> n; + clear(); + m_objects.resize(n); + + for_each(m_objects.begin(), m_objects.end(), ObjectReadFromStream(&in)); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +void CSetOfObjects::insert(const CVisualObject::Ptr& newObject) +{ + ASSERTMSG_(newObject.get() != this, "Error: Trying to insert container into itself!"); + m_objects.push_back(newObject); +} + +void CSetOfObjects::dumpListOfObjects(std::vector& lst) const +{ + for (auto& o : m_objects) + { + if (!o) continue; + // Single obj: + string s(o->GetRuntimeClass()->className); + if (!o->getName().empty()) s += string(" (") + o->getName() + string(")"); + lst.emplace_back(s); + + if (o->GetRuntimeClass() == CLASS_ID_NAMESPACE(CSetOfObjects, mrpt::viz)) + { + auto* objs = dynamic_cast(o.get()); + + std::vector auxLst; + objs->dumpListOfObjects(auxLst); + for (const auto& i : auxLst) lst.emplace_back(string(" ") + i); + } + } +} + +mrpt::containers::yaml CSetOfObjects::asYAML() const +{ + mrpt::containers::yaml d = mrpt::containers::yaml::Sequence(); + + d.asSequence().resize(m_objects.size()); + + for (uint32_t i = 0; i < m_objects.size(); i++) + { + const auto obj = m_objects.at(i); + mrpt::containers::yaml de = mrpt::containers::yaml::Map(); + + // class-specific properties: + obj->toYAMLMap(de); + + de["index"] = i; // type for "i" must be a stdint type + if (!obj) + { + de["class"] = "nullptr"; + continue; + } + de["class"] = obj->GetRuntimeClass()->className; + + // Single obj: + if (obj->GetRuntimeClass() == CLASS_ID_NAMESPACE(CSetOfObjects, mrpt::viz)) + { + de["obj_children"] = dynamic_cast(obj.get())->asYAML(); + } + d.asSequence().at(i) = std::move(de); + } + return d; +} + +/*-------------------------------------------------------------- + removeObject + ---------------------------------------------------------------*/ +void CSetOfObjects::removeObject(const CVisualObject::Ptr& obj) +{ + for (auto it = m_objects.begin(); it != m_objects.end(); ++it) + if (*it == obj) + { + m_objects.erase(it); + return; + } + else if ((*it)->GetRuntimeClass() == CLASS_ID_NAMESPACE(CSetOfObjects, mrpt::viz)) + dynamic_cast(it->get())->removeObject(obj); +} + +bool CSetOfObjects::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ + CPose3D nueva = (CPose3D() - getCPose()) + o; + bool found = false; + double tmp; + for (const auto& m_object : m_objects) + if (m_object->traceRay(nueva, tmp)) + { + if (!found) + { + found = true; + dist = tmp; + } + else if (tmp < dist) + dist = tmp; + } + return found; +} + +bool CSetOfObjects::contains(const CVisualObject::Ptr& obj) const +{ + return find(m_objects.begin(), m_objects.end(), obj) != m_objects.end(); +} + +CVisualObject& CSetOfObjects::setColor_u8(const mrpt::img::TColor& c) +{ + { + std::unique_lock lckWrite(m_stateMtx.data); + m_state.color = c; + } + for (auto& o : m_objects) + { + if (!o) continue; + o->setColor_u8(c); + } + return *this; +} + +CVisualObject& CSetOfObjects::setColorA_u8(const uint8_t a) +{ + { + std::unique_lock lckWrite(m_stateMtx.data); + m_state.color.A = a; + } + for (auto& o : m_objects) + { + if (!o) continue; + o->setColorA_u8(a); + } + return *this; +} + +/*--------------------------------------------------------------- + getByName + ---------------------------------------------------------------*/ +CVisualObject::Ptr CSetOfObjects::getByName(const string& str) +{ + for (auto& o : m_objects) + { + if (!o) continue; + if (o->getName() == str) + return o; + else if (auto objs = dynamic_cast(o.get())) + { + CVisualObject::Ptr ret = objs->getByName(str); + if (ret) return ret; + } + } + return {}; +} + +auto CSetOfObjects::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + mrpt::math::TBoundingBoxf bb; + bool first = true; + + for (const auto& o : m_objects) + { + if (!o) continue; + if (first) + { + bb = o->getBoundingBoxLocalf(); + first = false; + } + else + bb = bb.unionWith(o->getBoundingBoxLocalf()); + } + + return bb; +} + +CSetOfObjects::Ptr& mrpt::viz::operator<<(CSetOfObjects::Ptr& s, const CVisualObject::Ptr& r) +{ + s->insert(r); + return s; +} diff --git a/libs/viz/src/CSetOfTexturedTriangles.cpp b/libs/viz/src/CSetOfTexturedTriangles.cpp new file mode 100644 index 0000000000..6e858c9844 --- /dev/null +++ b/libs/viz/src/CSetOfTexturedTriangles.cpp @@ -0,0 +1,81 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace std; +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; + +IMPLEMENTS_SERIALIZABLE(CSetOfTexturedTriangles, CVisualObject, mrpt::viz) + +uint8_t CSetOfTexturedTriangles::serializeGetVersion() const { return 2; } +void CSetOfTexturedTriangles::serializeTo(mrpt::serialization::CArchive& out) const +{ + std::shared_lock readLock(m_trianglesMtx.data); + + uint32_t n; + + writeToStreamRender(out); + writeToStreamTexturedObject(out); + + n = (uint32_t)m_triangles.size(); + + out << n; + + for (uint32_t i = 0; i < n; i++) m_triangles[i].writeTo(out); +} + +void CSetOfTexturedTriangles::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + std::unique_lock writeLock(m_trianglesMtx.data); + + switch (version) + { + case 0: + case 1: + case 2: + { + readFromStreamRender(in); + if (version >= 2) + { + readFromStreamTexturedObject(in); + } + else + { // Old version. + THROW_EXCEPTION("deserializing old version not supported."); + } + + uint32_t n; + in >> n; + m_triangles.resize(n); + + for (uint32_t i = 0; i < n; i++) m_triangles[i].readFrom(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +bool CSetOfTexturedTriangles::traceRay( + [[maybe_unused]] const mrpt::poses::CPose3D& o, [[maybe_unused]] double& dist) const +{ + throw std::runtime_error("TODO: TraceRay not implemented in CSetOfTexturedTriangles"); +} + +auto CSetOfTexturedTriangles::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return trianglesBoundingBox(); +} diff --git a/libs/viz/src/CSetOfTriangles.cpp b/libs/viz/src/CSetOfTriangles.cpp new file mode 100644 index 0000000000..b8a03b71d4 --- /dev/null +++ b/libs/viz/src/CSetOfTriangles.cpp @@ -0,0 +1,151 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::poses; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CSetOfTriangles, CVisualObject, mrpt::viz) + +void CSetOfTriangles::clearTriangles() +{ + std::unique_lock trisWriteLock( + VisualObjectParams_Triangles::m_trianglesMtx.data); + auto& tris = VisualObjectParams_Triangles::m_triangles; + tris.clear(); + polygonsUpToDate = false; + CVisualObject::notifyChange(); +} + +size_t CSetOfTriangles::getTrianglesCount() const +{ + std::shared_lock trisReadLock( + VisualObjectParams_Triangles::m_trianglesMtx.data); + return shaderTrianglesBuffer().size(); +} + +uint8_t CSetOfTriangles::serializeGetVersion() const { return 0; } +void CSetOfTriangles::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + + std::shared_lock trisReadLock( + VisualObjectParams_Triangles::m_trianglesMtx.data); + + auto n = (uint32_t)shaderTrianglesBuffer().size(); + out << n; + for (size_t i = 0; i < n; i++) shaderTrianglesBuffer()[i].writeTo(out); +} +void CSetOfTriangles::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + std::unique_lock trisLck(VisualObjectParams_Triangles::m_trianglesMtx.data); + auto& tris = VisualObjectParams_Triangles::m_triangles; + + switch (version) + { + case 0: + { + readFromStreamRender(in); + uint32_t n; + in >> n; + tris.assign(n, TTriangle()); + for (size_t i = 0; i < n; i++) tris[i].readFrom(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + polygonsUpToDate = false; + CVisualObject::notifyChange(); +} + +bool CSetOfTriangles::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ + if (!polygonsUpToDate) updatePolygons(); + return mrpt::math::traceRay(m_polygons, (o - getCPose()).asTPose(), dist); +} +CVisualObject& CSetOfTriangles::setColor_u8(const mrpt::img::TColor& c) +{ + CVisualObject::notifyChange(); + setColor_u8(c); + std::unique_lock trisLck(VisualObjectParams_Triangles::m_trianglesMtx.data); + auto& tris = VisualObjectParams_Triangles::m_triangles; + + for (auto& t : tris) t.setColor(c); + return *this; +} + +CVisualObject& CSetOfTriangles::setColorA_u8(const uint8_t a) +{ + CVisualObject::notifyChange(); + setColorA_u8(a); + std::unique_lock trisLck(VisualObjectParams_Triangles::m_trianglesMtx.data); + auto& tris = VisualObjectParams_Triangles::m_triangles; + + for (auto& t : tris) t.setColor(getColor_u8()); + return *this; +} + +void CSetOfTriangles::getPolygons(std::vector& polys) const +{ + if (!polygonsUpToDate) updatePolygons(); + size_t N = m_polygons.size(); + for (size_t i = 0; i < N; i++) polys[i] = m_polygons[i].poly; +} + +void CSetOfTriangles::updatePolygons() const +{ + std::unique_lock trisLck(VisualObjectParams_Triangles::m_trianglesMtx.data); + auto& tris = VisualObjectParams_Triangles::m_triangles; + + TPolygon3D tmp(3); + size_t N = tris.size(); + m_polygons.resize(N); + for (size_t i = 0; i < N; i++) + for (size_t j = 0; j < 3; j++) + { + const TTriangle& t = tris[i]; + tmp[j].x = t.x(j); + tmp[j].y = t.y(j); + tmp[j].z = t.z(j); + m_polygons[i] = tmp; + } + polygonsUpToDate = true; + CVisualObject::notifyChange(); +} + +auto CSetOfTriangles::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return trianglesBoundingBox(); +} + +void CSetOfTriangles::insertTriangles(const CSetOfTriangles::Ptr& p) +{ + ASSERT_(p); + + std::unique_lock trisLck(VisualObjectParams_Triangles::m_trianglesMtx.data); + + std::shared_lock trisOtherLck(p->m_trianglesMtx.data); + + auto& tris = VisualObjectParams_Triangles::m_triangles; + auto& trisOther = p->shaderTrianglesBuffer(); + + reserve(tris.size() + trisOther.size()); + tris.insert(tris.end(), trisOther.begin(), trisOther.end()); + polygonsUpToDate = false; + CVisualObject::notifyChange(); +} diff --git a/libs/viz/src/CSimpleLine.cpp b/libs/viz/src/CSimpleLine.cpp new file mode 100644 index 0000000000..655909468a --- /dev/null +++ b/libs/viz/src/CSimpleLine.cpp @@ -0,0 +1,78 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CSimpleLine, CVisualObject, mrpt::viz) + +CSimpleLine::CSimpleLine( + float x0, + float y0, + float z0, + float x1, + float y1, + float z1, + float lineWidth, + bool antiAliasing) : + m_x0(x0), m_y0(y0), m_z0(z0), m_x1(x1), m_y1(y1), m_z1(z1) +{ + VisualObjectParams_Lines::setLineWidth(lineWidth); + VisualObjectParams_Lines::enableAntiAliasing(antiAliasing); +} + +uint8_t CSimpleLine::serializeGetVersion() const { return 2; } +void CSimpleLine::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_x0 << m_y0 << m_z0; + out << m_x1 << m_y1 << m_z1; + VisualObjectParams_Lines::params_serialize(out); // v2 +} + +void CSimpleLine::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 1: + case 2: + { + readFromStreamRender(in); + in >> m_x0 >> m_y0 >> m_z0; + in >> m_x1 >> m_y1 >> m_z1; + if (version >= 2) + { + VisualObjectParams_Lines::params_deserialize(in); // v2 + } + else + { + VisualObjectParams_Lines::setLineWidth(in.ReadAs()); + if (version >= 1) VisualObjectParams_Lines::enableAntiAliasing(in.ReadAs()); + } + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +auto CSimpleLine::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints( + {std::min(m_x0, m_x1), std::min(m_y0, m_y1), std::min(m_z0, m_z1)}, + {std::max(m_x0, m_x1), std::max(m_y0, m_y1), std::max(m_z0, m_z1)}); +} diff --git a/libs/viz/src/CSkyBox.cpp b/libs/viz/src/CSkyBox.cpp new file mode 100644 index 0000000000..f6d4ca60d9 --- /dev/null +++ b/libs/viz/src/CSkyBox.cpp @@ -0,0 +1,70 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CSkyBox, CVisualObject, mrpt::viz) + +uint8_t CSkyBox::serializeGetVersion() const { return 0; } +void CSkyBox::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_textureImages; // +} + +void CSkyBox::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + readFromStreamRender(in); + in >> m_textureImages; + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +/** In this class, returns a fixed box (max,max,max), (-max,-max,-max). */ +auto CSkyBox::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf { return {}; } + +void CSkyBox::assignImage(const CUBE_TEXTURE_FACE face, const mrpt::img::CImage& img) +{ + const int faceIdx = static_cast(face); + ASSERT_GE_(faceIdx, 0); + ASSERT_LT_(faceIdx, 6); + + m_textureImages[faceIdx] = img; + CVisualObject::notifyChange(); +} + +void CSkyBox::assignImage(const CUBE_TEXTURE_FACE face, mrpt::img::CImage&& img) +{ + const int faceIdx = static_cast(face); + ASSERT_GE_(faceIdx, 0); + ASSERT_LT_(faceIdx, 6); + + m_textureImages[faceIdx] = std::move(img); + CVisualObject::notifyChange(); +} + +CSkyBox::~CSkyBox() = default; diff --git a/libs/viz/src/CSphere.cpp b/libs/viz/src/CSphere.cpp new file mode 100644 index 0000000000..6329f1ba54 --- /dev/null +++ b/libs/viz/src/CSphere.cpp @@ -0,0 +1,98 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::poses; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CSphere, CVisualObject, mrpt::viz) + +uint8_t CSphere::serializeGetVersion() const { return 3; } +void CSphere::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_radius; + out << (uint32_t)m_nDivs; +} +void CSphere::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + case 3: + { + readFromStreamRender(in); + in >> m_radius; + m_nDivs = in.ReadAs(); + if (version < 3) + { + in.ReadAs(); // dummy old value, now unused + } + if (version == 1) + { + bool keepRadiusIndependentEyeDistance; + in >> keepRadiusIndependentEyeDistance; + } + + regenerateBaseParams(); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +bool CSphere::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ + // We need to find the points of the sphere which collide with the laser + // beam. + // The sphere's equation is invariant to rotations (but not to + // translations), and we can take advantage of this; + // we'll simply transform the center and then compute the beam's points + // whose distance to that transformed point + // equals the sphere's radius. + + CPose3D transf = getCPose() - o; + double x = transf.x(), y = transf.y(), z = transf.z(); + double r2 = m_radius * m_radius; + double dyz = y * y + z * z; + if (dyz > r2) return false; + double dx = sqrt(r2 - dyz); + if (x - dx >= 0) + { + dist = x - dx; + return true; + } + else if (x + dx >= 0) + { + dist = x + dx; + return true; + } + else + return false; +} + +auto CSphere::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + const float R = m_radius; + return { + {-R, -R, -R}, + { R, R, R} + }; +} diff --git a/libs/viz/src/CText.cpp b/libs/viz/src/CText.cpp new file mode 100644 index 0000000000..396c8976dc --- /dev/null +++ b/libs/viz/src/CText.cpp @@ -0,0 +1,92 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CText, CVisualObject, mrpt::viz) + +CText::~CText() = default; + +#if 0 +constexpr double text_spacing = 1.5; +constexpr double text_kerning = 0.1; +#endif + +std::pair CText::computeTextExtension() const +{ +#if 0 + mrpt::viz::internal::glSetFont(m_fontName); + const auto [textW, textH] = mrpt::viz::internal::glGetExtends(m_str, text_spacing, text_kerning); + return {textW, textH}; +#endif + THROW_EXCEPTION("TODO"); + MRPT_TODO("Refactor!"); +} + +uint8_t CText::serializeGetVersion() const { return 2; } +void CText::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_str; + out << m_fontName; + out << (uint32_t)m_fontHeight; +} + +void CText::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + { + uint32_t i; + readFromStreamRender(in); + in >> m_str; + if (version >= 1) + { + in >> m_fontName; + in >> i; + m_fontHeight = i; + + if (version < 2) + { + in >> i; + // dummy, removed in v2: m_fontWidth = i; + } + } + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +auto CText::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return { + {0.f, 0.f, 0.f}, + {0.f, 0.f, 0.f} + }; +} + +void CText::toYAMLMap(mrpt::containers::yaml& propertiesMap) const +{ + CVisualObject::toYAMLMap(propertiesMap); + propertiesMap["text"] = m_str; +} diff --git a/libs/viz/src/CText3D.cpp b/libs/viz/src/CText3D.cpp new file mode 100644 index 0000000000..bd6d9e5fd7 --- /dev/null +++ b/libs/viz/src/CText3D.cpp @@ -0,0 +1,77 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CText3D, CVisualObject, mrpt::viz) + +CText3D::CText3D( + const std::string& str, + const std::string& fontName, + const float scale, + const mrpt::viz::TOpenGLFontStyle text_style, + const double text_spacing, + const double text_kerning) : + m_str(str), + m_fontName(fontName), + m_text_style(text_style), + m_text_spacing(text_spacing), + m_text_kerning(text_kerning) +{ + this->setScale(scale); +} + +CText3D::~CText3D() = default; + +uint8_t CText3D::serializeGetVersion() const { return 0; } +void CText3D::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << m_str << m_fontName << (uint32_t)m_text_style << m_text_spacing << m_text_kerning; +} + +void CText3D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + readFromStreamRender(in); + + uint32_t i; + in >> m_str >> m_fontName >> i >> m_text_spacing >> m_text_kerning; + + m_text_style = TOpenGLFontStyle(i); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +auto CText3D::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints( + {0.f, 0.f, 0.f}, {m_str.size() * getScaleX(), 1.0f * getScaleY(), 0.f}); +} + +void CText3D::toYAMLMap(mrpt::containers::yaml& propertiesMap) const +{ + CVisualObject::toYAMLMap(propertiesMap); + propertiesMap["text"] = m_str; +} diff --git a/libs/viz/src/CTextMessageCapable.cpp b/libs/viz/src/CTextMessageCapable.cpp new file mode 100644 index 0000000000..c1f1e65ef2 --- /dev/null +++ b/libs/viz/src/CTextMessageCapable.cpp @@ -0,0 +1,84 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include + + +using namespace std; +using namespace mrpt; +using namespace mrpt::viz; + +void CTextMessageCapable::TListTextMessages::regenerateGLobjects() const +{ + std::unique_lock lckWrite2DTexts(mtx.data); + + // (re)generate the opengl CText objects for each label: + for (auto& kv : messages) + { + const DataPerText& labelData = kv.second; + if (labelData.gl_text && labelData.gl_text_outdated) continue; + + if (!labelData.gl_text) + { + labelData.gl_text = mrpt::viz::CText::Create(); + } + if (labelData.draw_shadow && !labelData.gl_text_shadow) + labelData.gl_text_shadow = mrpt::viz::CText::Create(); + + if (!labelData.draw_shadow && labelData.gl_text_shadow) labelData.gl_text_shadow.reset(); + + kv.second.gl_text_outdated = false; + } +} + +void CTextMessageCapable::clearTextMessages() +{ + std::unique_lock lckWrite2DTexts(m_2D_texts.mtx.data); + m_2D_texts.messages.clear(); +} + +/** Just updates the text of a given text message, without touching the other + * parameters. + * \return false if given ID doesn't exist. + */ +bool CTextMessageCapable::updateTextMessage(size_t unique_index, const std::string& text) +{ + std::unique_lock lckWrite2DTexts(m_2D_texts.mtx.data); + + auto it = m_2D_texts.messages.find(unique_index); + if (it == m_2D_texts.messages.end()) + return false; + else + { + it->second.text = text; + it->second.gl_text_outdated = true; + return true; + } +} + +/// overload with more font parameters - refer to +/// mrpt::viz::gl_utils::glDrawText() +void CTextMessageCapable::addTextMessage( + const double x_frac, + const double y_frac, + const std::string& text, + size_t unique_index, + const TFontParams& fontParams) +{ + DataPerText d; + static_cast(d) = fontParams; + d.text = text; + d.x = x_frac; + d.y = y_frac; + + std::unique_lock lckWrite2DTexts(m_2D_texts.mtx.data); + m_2D_texts.messages[unique_index] = std::move(d); +} diff --git a/libs/viz/src/CTexturedPlane.cpp b/libs/viz/src/CTexturedPlane.cpp new file mode 100644 index 0000000000..5ebd1aa40b --- /dev/null +++ b/libs/viz/src/CTexturedPlane.cpp @@ -0,0 +1,87 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::poses; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CTexturedPlane, CVisualObject, mrpt::viz) + +CTexturedPlane::CTexturedPlane(float x_min, float x_max, float y_min, float y_max) +{ + VisualObjectParams_Triangles::enableLight(false); + VisualObjectParams_TexturedTriangles::enableLight(false); + + setPlaneCorners(x_min, x_max, y_min, y_max); +} +uint8_t CTexturedPlane::serializeGetVersion() const { return 2; } +void CTexturedPlane::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + + out << m_xMin << m_xMax; + out << m_yMin << m_yMax; + + writeToStreamTexturedObject(out); +} + +void CTexturedPlane::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + THROW_EXCEPTION("Deserialization of old formats not supported."); + break; + case 2: + { + readFromStreamRender(in); + in >> m_xMin >> m_xMax; + in >> m_yMin >> m_yMax; + readFromStreamTexturedObject(in); + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + CVisualObject::notifyChange(); +} + +bool CTexturedPlane::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ + if (!polygonUpToDate) updatePoly(); + return math::traceRay(tmpPoly, (o - getCPose()).asTPose(), dist); +} + +void CTexturedPlane::updatePoly() const +{ + TPolygon3D poly(4); + poly[0].x = poly[1].x = m_xMin; + poly[2].x = poly[3].x = m_xMax; + poly[0].y = poly[3].y = m_yMin; + poly[1].y = poly[2].y = m_yMax; + for (size_t i = 0; i < 4; i++) poly[i].z = 0; + tmpPoly.resize(1); + tmpPoly[0] = poly; + polygonUpToDate = true; +} + +auto CTexturedPlane::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return mrpt::math::TBoundingBoxf::FromUnsortedPoints( + {m_xMin, m_yMin, 0.f}, {m_xMax, m_yMax, 0.f}); +} diff --git a/libs/viz/src/CVectorField2D.cpp b/libs/viz/src/CVectorField2D.cpp new file mode 100644 index 0000000000..90881b5177 --- /dev/null +++ b/libs/viz/src/CVectorField2D.cpp @@ -0,0 +1,108 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CVectorField2D, CVisualObject, mrpt::viz) + +/** Constructor */ +CVectorField2D::CVectorField2D() : xcomp(0, 0), ycomp(0, 0) +{ + m_point_color = getColor_u8(); + m_field_color = getColor_u8(); +} + +/** Constructor with a initial set of lines. */ +CVectorField2D::CVectorField2D( + [[maybe_unused]] CMatrixFloat Matrix_x, + [[maybe_unused]] CMatrixFloat Matrix_y, + [[maybe_unused]] float xmin, + [[maybe_unused]] float xmax, + [[maybe_unused]] float ymin, + [[maybe_unused]] float ymax) +{ + m_point_color = getColor_u8(); + m_field_color = getColor_u8(); +} + +/*--------------------------------------------------------------- + Implements the writing to a CStream capability of + CSerializable objects + ---------------------------------------------------------------*/ +uint8_t CVectorField2D::serializeGetVersion() const { return 2; } +void CVectorField2D::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + out << xcomp << ycomp; + out << xMin << xMax << yMin << yMax; + out << m_point_color; + out << m_field_color; + VisualObjectParams_Lines::params_serialize(out); + VisualObjectParams_Points::params_serialize(out); + VisualObjectParams_Triangles::params_serialize(out); +} + +void CVectorField2D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + THROW_EXCEPTION("Unsupported old serialized version"); + break; + + case 2: + readFromStreamRender(in); + + in >> xcomp >> ycomp; + in >> xMin >> xMax >> yMin >> yMax; + in >> m_point_color; + in >> m_field_color; + + VisualObjectParams_Lines::params_deserialize(in); + VisualObjectParams_Points::params_deserialize(in); + VisualObjectParams_Triangles::params_deserialize(in); + + break; + + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + break; + }; + CVisualObject::notifyChange(); +} + +auto CVectorField2D::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return verticesBoundingBox(); +} + +void CVectorField2D::adjustVectorFieldToGrid() +{ + ASSERT_(xcomp.size() > 0); + + const float ratio_xp = xcomp.maxCoeff() * (xcomp.cols() - 1) / (xMax - xMin); + const float ratio_xn = xcomp.minCoeff() * (xcomp.cols() - 1) / (xMax - xMin); + const float ratio_yp = ycomp.maxCoeff() * (ycomp.rows() - 1) / (yMax - yMin); + const float ratio_yn = ycomp.minCoeff() * (ycomp.rows() - 1) / (yMax - yMin); + const float norm_factor = + 0.85f / max(max(ratio_xp, std::abs(ratio_xn)), max(ratio_yp, std::abs(ratio_yn))); + + xcomp *= norm_factor; + ycomp *= norm_factor; + CVisualObject::notifyChange(); +} diff --git a/libs/viz/src/CVectorField3D.cpp b/libs/viz/src/CVectorField3D.cpp new file mode 100644 index 0000000000..02d72c9173 --- /dev/null +++ b/libs/viz/src/CVectorField3D.cpp @@ -0,0 +1,91 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(CVectorField3D, CVisualObject, mrpt::viz) + +/** Constructor */ +CVectorField3D::CVectorField3D() : + x_vf(0, 0), y_vf(0, 0), z_vf(0, 0), x_p(0, 0), y_p(0, 0), z_p(0, 0) +{ + m_point_color = m_field_color = m_still_color = m_maxspeed_color = getColor_u8(); + m_maxspeed = 1.f; +} + +/** Constructor with a initial set of lines. */ +CVectorField3D::CVectorField3D( + CMatrixFloat x_vf_ini, + CMatrixFloat y_vf_ini, + CMatrixFloat z_vf_ini, + CMatrixFloat x_p_ini, + CMatrixFloat y_p_ini, + CMatrixFloat z_p_ini) : + m_colorFromModule(false), m_showPoints(true) +{ + x_vf = x_vf_ini; + y_vf = y_vf_ini; + z_vf = z_vf_ini; + x_p = x_p_ini; + y_p = y_p_ini; + z_p = z_p_ini; + m_point_color = m_field_color = m_still_color = m_maxspeed_color = getColor_u8(); + m_maxspeed = 1.f; +} + +uint8_t CVectorField3D::serializeGetVersion() const { return 1; } +void CVectorField3D::serializeTo(mrpt::serialization::CArchive& out) const +{ + writeToStreamRender(out); + + out << x_vf << y_vf << z_vf; + out << x_p << y_p << z_p; + out << m_point_color; + out << m_field_color; + VisualObjectParams_Lines::params_serialize(out); // v1 + VisualObjectParams_Points::params_serialize(out); // v1 +} +void CVectorField3D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + THROW_EXCEPTION("Importing from old version not supported"); + break; + + case 1: + readFromStreamRender(in); + + in >> x_vf >> y_vf >> z_vf; + in >> x_p >> y_p >> z_p; + in >> m_point_color; + in >> m_field_color; + VisualObjectParams_Lines::params_deserialize(in); // v1 + VisualObjectParams_Points::params_deserialize(in); // v1 + break; + + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + break; + }; + CVisualObject::notifyChange(); +} + +auto CVectorField3D::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf +{ + return verticesBoundingBox(); +} diff --git a/libs/viz/src/CVisualObject.cpp b/libs/viz/src/CVisualObject.cpp new file mode 100644 index 0000000000..6b5efb5409 --- /dev/null +++ b/libs/viz/src/CVisualObject.cpp @@ -0,0 +1,530 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include +#include +#include +#include +#include +#include // Include these before windows.h!! + +#include + +using namespace std; +using namespace mrpt; +using namespace mrpt::viz; + +IMPLEMENTS_VIRTUAL_SERIALIZABLE(CVisualObject, CSerializable, mrpt::viz) + +// Destructor: +CVisualObject::~CVisualObject() = default; + +void CVisualObject::writeToStreamRender(mrpt::serialization::CArchive& out) const +{ + std::shared_lock lckRead(m_stateMtx.data); + const auto& _ = m_state; + + // MRPT 0.9.5 svn 2774 (Dec 14th 2011): + // Added support of versioning at this level of serialization too. + // Should have been done from the beginning, terrible mistake on my part. + // Now, the only solution is something as ugly as this: + // + // For reference: In the past this started as: + // out << m_name << (float)(m_color.R) << (float)(m_color.G) << + // (float)(m_color.B) << (float)(m_color.A); + // ... + + // can't be >31 (but it would be mad getting to that situation!) + const uint8_t serialization_version = 2; + + const bool all_scales_equal = (_.scale_x == _.scale_y && _.scale_z == _.scale_x); + const bool all_scales_unity = (all_scales_equal && _.scale_x == 1.0f); + + const uint8_t magic_signature[2] = { + 0xFF, + // bit7: fixed to 1 to mark this new header format + // bit6: whether the 3 scale{x,y,z} are equal to 1.0 + // bit5: whether the 3 scale{x,y,z} are equal to each other + static_cast( + serialization_version | (all_scales_unity ? 0xC0 : (all_scales_equal ? 0xA0 : 0x80)))}; + + out << magic_signature[0] << magic_signature[1]; + + // "m_name" + const auto nameLen = static_cast(_.name.size()); + out << nameLen; + if (nameLen) out.WriteBuffer(_.name.c_str(), _.name.size()); + + // Color, as u8: + out << _.color.R << _.color.G << _.color.B << _.color.A; + + // the rest of fields: + out << (float)_.pose.x() << (float)_.pose.y() << (float)_.pose.z() << (float)_.pose.yaw() + << (float)_.pose.pitch() << (float)_.pose.roll(); + + if (!all_scales_unity) + { + if (all_scales_equal) + out << _.scale_x; + else + out << _.scale_x << _.scale_y << _.scale_z; + } + + out << _.show_name << _.visible; + out << _.representativePoint; // v1 + out << _.materialShininess << _.castShadows; // v2 +} + +void CVisualObject::readFromStreamRender(mrpt::serialization::CArchive& in) +{ + // MRPT 0.9.5 svn 2774 (Dec 14th 2011): + // See comments in CVisualObject::writeToStreamRender() for the employed + // serialization mechanism. + // + // MRPT 1.9.9 (Aug 2019): Was + // union { + // uint8_t magic_signature[2 + 2]; + // (the extra 4 bytes will be used only for the old format) + // uint32_t magic_signature_uint32; + // So we can interpret the 4bytes above as a 32bit number cleanly. + // }; + // Get rid of the "old" serialization format to avoid using "union". + + uint8_t magic_signature[2]; + + in >> magic_signature[0] >> magic_signature[1]; + + const bool is_new_format = (magic_signature[0] == 0xFF) && ((magic_signature[1] & 0x80) != 0); + + std::unique_lock lckWrite(m_stateMtx.data); + auto& _ = m_state; + + if (is_new_format) + { + // NEW FORMAT: + uint8_t serialization_version = (magic_signature[1] & 0x1F); + const bool all_scales_unity = ((magic_signature[1] & 0x40) != 0); + const bool all_scales_equal_but_not_unity = ((magic_signature[1] & 0x20) != 0); + + switch (serialization_version) + { + case 0: + case 1: + case 2: + { + // "m_name" + uint16_t nameLen; + in >> nameLen; + _.name.resize(nameLen); + if (nameLen) in.ReadBuffer((void*)(&_.name[0]), _.name.size()); + + // Color, as u8: + in >> _.color.R >> _.color.G >> _.color.B >> _.color.A; + + // the rest of fields: + float x, y, z, yaw, pitch, roll; + in >> x >> y >> z >> yaw >> pitch >> roll; + _.pose.x(x); + _.pose.y(y); + _.pose.z(z); + _.pose.setYawPitchRoll(yaw, pitch, roll); + + if (all_scales_unity) + _.scale_x = _.scale_y = _.scale_z = 1; + else + { + if (all_scales_equal_but_not_unity) + { + in >> _.scale_x; + _.scale_y = _.scale_z = _.scale_x; + } + else + in >> _.scale_x >> _.scale_y >> _.scale_z; + } + + in >> _.show_name >> _.visible; + if (serialization_version >= 1) + in >> _.representativePoint; + else + _.representativePoint = mrpt::math::TPoint3Df(0, 0, 0); + + if (serialization_version >= 2) + in >> _.materialShininess >> _.castShadows; + else + { + // default + _.materialShininess = 0.2f; + _.castShadows = true; + } + } + break; + default: + THROW_EXCEPTION_FMT( + "Can't parse CVisualObject standard data field: corrupt " + "data stream or format in a newer MRPT format? " + "(serialization version=%u)", + static_cast(serialization_version)); + }; + } + else + { + // OLD FORMAT: + THROW_EXCEPTION("Serialized object is too old! Unsupported format."); + } +} + +/*-------------------------------------------------------------- + setPose + ---------------------------------------------------------------*/ +CVisualObject& CVisualObject::setPose(const mrpt::poses::CPose3D& o) +{ + m_stateMtx.data.lock(); + m_state.pose = o; + m_stateMtx.data.unlock(); + return *this; +} +CVisualObject& CVisualObject::setPose(const mrpt::poses::CPose2D& o) +{ + return setPose(mrpt::poses::CPose3D(o)); +} +CVisualObject& CVisualObject::setPose(const mrpt::math::TPose3D& o) +{ + return setPose(mrpt::poses::CPose3D(o)); +} +CVisualObject& CVisualObject::setPose(const mrpt::math::TPose2D& o) +{ + return setPose(mrpt::poses::CPose3D(o)); +} + +CVisualObject& CVisualObject::setPose(const mrpt::poses::CPoint3D& o) +{ + m_stateMtx.data.lock(); + m_state.pose.setFromValues(o.x(), o.y(), o.z(), 0, 0, 0); + m_stateMtx.data.unlock(); + return *this; +} +CVisualObject& CVisualObject::setPose(const mrpt::poses::CPoint2D& o) +{ + m_stateMtx.data.lock(); + m_state.pose.setFromValues(o.x(), o.y(), 0, 0, 0, 0); + m_stateMtx.data.unlock(); + return *this; +} + +mrpt::math::TPose3D CVisualObject::getPose() const +{ + std::shared_lock lckRead(m_stateMtx.data); + return m_state.pose.asTPose(); +} +bool CVisualObject::traceRay(const mrpt::poses::CPose3D&, double&) const { return false; } + +CVisualObject& CVisualObject::setColor_u8(const mrpt::img::TColor& c) +{ + std::unique_lock lckWrite(m_stateMtx.data); + m_state.color.R = c.R; + m_state.color.G = c.G; + m_state.color.B = c.B; + m_state.color.A = c.A; + notifyChange(); + return *this; +} + +CText& CVisualObject::labelObject() const +{ + if (!m_label_obj) + { + m_label_obj = std::make_shared(); + m_label_obj->setString(getName()); + } + return *m_label_obj; +} + +void CVisualObject::toYAMLMap(mrpt::containers::yaml& propertiesMap) const +{ + propertiesMap["name"] = getName(); + propertiesMap["show_name"] = isShowNameEnabled(); + propertiesMap["location"] = getPose().asString(); + propertiesMap["visible"] = isVisible(); +} + +#ifdef MRPT_OPENGL_PROFILER +mrpt::system::CTimeLogger& mrpt::viz::opengl_profiler() +{ + static mrpt::system::CTimeLogger tl; + return tl; +} +#endif + +auto CVisualObject::getBoundingBoxLocalf() const -> mrpt::math::TBoundingBoxf +{ + if (!m_cachedLocalBBox) + { + std::unique_lock lckWrite(m_outdatedStateMtx.data); + m_cachedLocalBBox = internalBoundingBoxLocal(); + return m_cachedLocalBBox.value(); + } + else + { + std::shared_lock lckWrite(m_outdatedStateMtx.data); + return m_cachedLocalBBox.value(); + } +} + +auto CVisualObject::getBoundingBoxLocal() const -> mrpt::math::TBoundingBox +{ + const auto& bb = getBoundingBoxLocalf(); + return {bb.min.cast(), bb.max.cast()}; +} + +void VisualObjectParams_Triangles::params_serialize(mrpt::serialization::CArchive& out) const +{ + out.WriteAs(0); // serialization version + out << m_enableLight << static_cast(m_cullface); +} +void VisualObjectParams_Triangles::params_deserialize(mrpt::serialization::CArchive& in) +{ + const auto version = in.ReadAs(); + + switch (version) + { + case 0: + in >> m_enableLight; + m_cullface = static_cast(in.ReadAs()); + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +const math::TBoundingBoxf VisualObjectParams_Triangles::trianglesBoundingBox() const +{ + mrpt::math::TBoundingBoxf bb; + + std::shared_lock readLock(m_trianglesMtx.data); + + if (m_triangles.empty()) return bb; + + bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); + + for (const auto& t : m_triangles) + for (int i = 0; i < 3; i++) bb.updateWithPoint(t.vertices[i].xyzrgba.pt); + + return bb; +} + +void VisualObjectParams_Lines::params_serialize(serialization::CArchive& out) const +{ + out.WriteAs(0); // serialization version + out << m_lineWidth << m_antiAliasing; +} + +void VisualObjectParams_Lines::params_deserialize(serialization::CArchive& in) +{ + const auto version = in.ReadAs(); + + switch (version) + { + case 0: + in >> m_lineWidth >> m_antiAliasing; + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +void VisualObjectParams_Points::params_serialize(serialization::CArchive& out) const +{ + out.WriteAs(0); // serialization version + out << m_pointSize << m_variablePointSize << m_variablePointSize_K + << m_variablePointSize_DepthScale; +} + +void VisualObjectParams_Points::params_deserialize(serialization::CArchive& in) +{ + const auto version = in.ReadAs(); + + switch (version) + { + case 0: + in >> m_pointSize >> m_variablePointSize >> m_variablePointSize_K >> + m_variablePointSize_DepthScale; + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +const math::TBoundingBoxf VisualObjectParams_Points::verticesBoundingBox() const +{ + std::shared_lock wfReadLock(VisualObjectParams_Points::m_pointsMtx.data); + + mrpt::math::TBoundingBoxf bb; + + if (m_vertex_buffer_data.empty()) return bb; + + bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); + + for (const auto& p : m_vertex_buffer_data) bb.updateWithPoint(p); + + return bb; +} + +const mrpt::math::TBoundingBoxf VisualObjectParams_TexturedTriangles::trianglesBoundingBox() const +{ + mrpt::math::TBoundingBoxf bb; + + std::shared_lock readLock(m_trianglesMtx.data); + + if (m_triangles.empty()) return bb; + + bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); + + for (const auto& t : m_triangles) + for (int i = 0; i < 3; i++) bb.updateWithPoint(t.vertices[i].xyzrgba.pt); + + return bb; +} + +void VisualObjectParams_TexturedTriangles::writeToStreamTexturedObject( + serialization::CArchive& out) const +{ + uint8_t ver = 3; + + out << ver; + out << m_enableTransparency << m_textureInterpolate << m_textureUseMipMaps; + out << m_textureImage; + if (m_enableTransparency) out << m_textureImageAlpha; + out << m_textureImageAssigned; + out << m_enableLight << static_cast(m_cullface); // v2 +} + +void VisualObjectParams_TexturedTriangles::readFromStreamTexturedObject(serialization::CArchive& in) +{ + uint8_t version; + in >> version; + + switch (version) + { + case 0: + case 1: + case 2: + case 3: + { + in >> m_enableTransparency >> m_textureInterpolate; + if (version >= 3) + { + in >> m_textureUseMipMaps; + } + else + { + m_textureUseMipMaps = true; + } + in >> m_textureImage; + if (m_enableTransparency) + { + in >> m_textureImageAlpha; + assignImage(m_textureImage, m_textureImageAlpha); + } + else + { + assignImage(m_textureImage); + } + if (version >= 1) + in >> m_textureImageAssigned; + else + m_textureImageAssigned = true; + + if (version >= 2) + { + in >> m_enableLight; + m_cullface = static_cast(in.ReadAs()); + } + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + + CVisualObject::notifyChange(); +} + +void VisualObjectParams_TexturedTriangles::assignImage( + const mrpt::img::CImage& img, const mrpt::img::CImage& imgAlpha) +{ + MRPT_START + + CVisualObject::notifyChange(); + + // m_glTexture.unloadTexture(); + + // Make a copy: + m_textureImage = img; + m_textureImageAlpha = imgAlpha; + m_textureImageAssigned = true; + + m_enableTransparency = true; + + MRPT_END +} + +void VisualObjectParams_TexturedTriangles::assignImage(const mrpt::img::CImage& img) +{ + MRPT_START + + CVisualObject::notifyChange(); + + // m_glTexture.unloadTexture(); + + // Make a shallow copy: + m_textureImage = img; + m_textureImageAssigned = true; + + m_enableTransparency = false; + + MRPT_END +} + +void VisualObjectParams_TexturedTriangles::assignImage( + mrpt::img::CImage&& img, mrpt::img::CImage&& imgAlpha) +{ + MRPT_START + + CVisualObject::notifyChange(); + + // m_glTexture.unloadTexture(); + + m_textureImage = std::move(img); + m_textureImageAlpha = std::move(imgAlpha); + m_textureImageAssigned = true; + + m_enableTransparency = true; + + MRPT_END +} + +void VisualObjectParams_TexturedTriangles::assignImage(mrpt::img::CImage&& img) +{ + MRPT_START + + CVisualObject::notifyChange(); + + // mrpt::img::m_glTexture.unloadTexture(); + + m_textureImage = std::move(img); + m_textureImageAssigned = true; + + m_enableTransparency = false; + + MRPT_END +} diff --git a/libs/opengl/src/PLY_import_export.cpp b/libs/viz/src/PLY_import_export.cpp similarity index 99% rename from libs/opengl/src/PLY_import_export.cpp rename to libs/viz/src/PLY_import_export.cpp index 9dcbb4fa6b..7beabaa5ca 100644 --- a/libs/opengl/src/PLY_import_export.cpp +++ b/libs/viz/src/PLY_import_export.cpp @@ -45,12 +45,12 @@ WARRANTY OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. */ -#include "opengl-precomp.h" // Precompiled headers +#include "viz-precomp.h" // Precompiled headers // #include #include -#include #include +#include #include #include @@ -58,7 +58,7 @@ WARRANTY OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. using namespace std; using namespace mrpt; -using namespace mrpt::opengl; +using namespace mrpt::viz; using namespace mrpt::math; using namespace mrpt::img; diff --git a/libs/viz/src/Scene.cpp b/libs/viz/src/Scene.cpp new file mode 100644 index 0000000000..23cc007be5 --- /dev/null +++ b/libs/viz/src/Scene.cpp @@ -0,0 +1,295 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include +#include + +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::serialization::metaprogramming; +using namespace mrpt::math; +using namespace std; + +// Include libraries in linking: +#if MRPT_HAS_OPENGL_GLUT || MRPT_HAS_EGL +#ifdef _WIN32 +// WINDOWS: +#if defined(_MSC_VER) +#pragma comment(lib, "opengl32.lib") +#pragma comment(lib, "GlU32.lib") +#endif +#endif // _WIN32 +#endif // MRPT_HAS_OPENGL_GLUT + +IMPLEMENTS_SERIALIZABLE(Scene, CSerializable, mrpt::viz) + +/*--------------------------------------------------------------- + Constructor +---------------------------------------------------------------*/ +Scene::Scene() { createViewport("main"); } +/*-------------------------------------------------------------- + Copy constructor + ---------------------------------------------------------------*/ +Scene::Scene(const Scene& obj) : CSerializable() { (*this) = obj; } + +Scene::~Scene() { m_viewports.clear(); } + +void Scene::clear(bool createMainViewport) +{ + m_viewports.clear(); + + if (createMainViewport) createViewport("main"); +} + +/*--------------------------------------------------------------- + = + ---------------------------------------------------------------*/ +Scene& Scene::operator=(const Scene& obj) +{ + if (this != &obj) + { + m_followCamera = obj.m_followCamera; + + clear(); + m_viewports = obj.m_viewports; + for_each( + m_viewports.begin(), m_viewports.end(), + [](auto& ptr) + { + // make a unique copy of each object (copied as a shared ptr) + ptr.reset(dynamic_cast(ptr->clone())); + }); + } + return *this; +} + +uint8_t Scene::serializeGetVersion() const { return 1; } +void Scene::serializeTo(mrpt::serialization::CArchive& out) const +{ + out << m_followCamera; + + uint32_t n; + n = (uint32_t)m_viewports.size(); + out << n; + for (const auto& m_viewport : m_viewports) out << *m_viewport; +} + +void Scene::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + // Old style: Just one viewport: + clear(true); + Viewport::Ptr view = m_viewports[0]; + + // Load objects: + uint32_t n; + in >> n; + + view->clear(); + view->m_objects.resize(n); + for_each(view->m_objects.begin(), view->m_objects.end(), ObjectReadFromStream(&in)); + } + break; + case 1: + { + in >> m_followCamera; + + uint32_t i, n; + in >> n; + clear(false); + + for (i = 0; i < n; i++) + { + CSerializable::Ptr newObj; + in >> newObj; + + Viewport::Ptr newView = std::dynamic_pointer_cast(newObj); + newView->m_parent = this; + m_viewports.push_back(newView); + } + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +/*--------------------------------------------------------------- + insert + ---------------------------------------------------------------*/ +void Scene::insert(const CVisualObject::Ptr& newObject, const std::string& viewportName) +{ + MRPT_START + for (auto& m_viewport : m_viewports) + { + if (m_viewport->m_name == viewportName) + { + m_viewport->insert(newObject); + return; + } + } + THROW_EXCEPTION_FMT("Error: viewport '%s' not found.", viewportName.c_str()); + MRPT_END +} + +/*--------------------------------------------------------------- + getByName + ---------------------------------------------------------------*/ +CVisualObject::Ptr Scene::getByName(const string& str, [[maybe_unused]] const string& viewportName) +{ + CVisualObject::Ptr obj; + for (auto& m_viewport : m_viewports) + if ((obj = m_viewport->getByName(str))) break; + return obj; +} + +/*-------------------------------------------------------------- + dumpListOfObjects + ---------------------------------------------------------------*/ +void Scene::dumpListOfObjects(std::vector& lst) const +{ + using namespace std::string_literals; + lst.clear(); + + for (auto& v : m_viewports) + { + lst.emplace_back("Viewport: '"s + v->m_name + "'"s); + lst.emplace_back("============================================"); + v->dumpListOfObjects(lst); + } +} + +mrpt::containers::yaml Scene::asYAML() const +{ + mrpt::containers::yaml d = mrpt::containers::yaml::Map(); + auto vs = d["viewports"]; + + for (auto& v : m_viewports) vs[v->m_name] = v->asYAML(); + + return d; +} + +/*-------------------------------------------------------------- + createViewport + ---------------------------------------------------------------*/ +Viewport::Ptr Scene::createViewport(const string& viewportName) +{ + MRPT_START + + Viewport::Ptr old = getViewport(viewportName); + if (old) return old; + + auto theNew = std::make_shared(this, viewportName); + m_viewports.push_back(theNew); + return theNew; + + MRPT_END +} + +/*-------------------------------------------------------------- + getViewport + ---------------------------------------------------------------*/ +Viewport::Ptr Scene::getViewport(const std::string& viewportName) const +{ + MRPT_START + for (const auto& m_viewport : m_viewports) + if (m_viewport->m_name == viewportName) return m_viewport; + return Viewport::Ptr(); + MRPT_END +} + +/*-------------------------------------------------------------- + removeObject + ---------------------------------------------------------------*/ +void Scene::removeObject(const CVisualObject::Ptr& obj, const std::string& viewportName) +{ + MRPT_START + + Viewport::Ptr view = getViewport(viewportName); + ASSERT_(view); + view->removeObject(obj); + + MRPT_END +} + +bool Scene::traceRay(const mrpt::poses::CPose3D& o, double& dist) const +{ + bool found = false; + double tmp; + for (const auto& vp : m_viewports) + { + for (auto it2 = vp->m_objects.begin(); it2 != vp->m_objects.end(); ++it2) + if ((*it2)->traceRay(o, tmp)) + { + if (!found) + { + found = true; + dist = tmp; + } + else if (tmp < dist) + dist = tmp; + } + } + return found; +} + +bool Scene::saveToFile(const std::string& fil) const +{ + try + { + mrpt::io::CFileGZOutputStream f(fil); + mrpt::serialization::archiveFrom(f) << *this; + return true; + } + catch (...) + { + return false; + } +} + +bool Scene::loadFromFile(const std::string& fil) +{ + try + { + mrpt::io::CFileGZInputStream f(fil); + mrpt::serialization::archiveFrom(f) >> *this; + return true; + } + catch (const std::exception& e) + { + std::cerr << "[mrpt::viz::Scene] Error loading '" << fil << "': " << e.what() << std::endl; + return false; + } +} + +/** Evaluates the bounding box of this object (including possible children) in + * the coordinate frame of the object parent. */ +auto Scene::getBoundingBox(const std::string& vpn) const -> mrpt::math::TBoundingBox +{ + Viewport::Ptr vp = this->getViewport(vpn); + ASSERTMSG_(vp, "No opengl viewport exists with the given name"); + + return vp->getBoundingBox(); +} + +Scene::Ptr& mrpt::viz::operator<<(Scene::Ptr& s, const CVisualObject::Ptr& r) +{ + s->insert(r); + return s; +} diff --git a/libs/viz/src/StockObjects.cpp b/libs/viz/src/StockObjects.cpp new file mode 100644 index 0000000000..313c69bc05 --- /dev/null +++ b/libs/viz/src/StockObjects.cpp @@ -0,0 +1,507 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace mrpt::viz; +using namespace mrpt::poses; +using namespace mrpt::math; +using namespace std; + +/*--------------------------------------------------------------- + RobotPioneer + ---------------------------------------------------------------*/ +CSetOfObjects::Ptr stock_objects::RobotPioneer() +{ + CSetOfObjects::Ptr ret = std::make_shared(); + + ret->setName("theRobot"); + + CSetOfTriangles::Ptr obj = std::make_shared(); + obj->setLocalRepresentativePoint({0.0f, 0.0f, 0.15f}); + + // Add triangles: + mrpt::viz::TTriangle t; + + t.r(0) = t.r(1) = t.r(2) = 255; + t.g(0) = t.g(1) = t.g(2) = 0; + t.b(0) = t.b(1) = t.b(2) = 0; + t.a(0) = t.a(1) = t.a(2) = 255; + + t.vertex(0) = {0.10f, -0.10f, 0.20f}; + t.vertex(1) = {-0.20f, 0.10f, 0.25f}; + t.vertex(2) = {-0.20f, -0.10f, 0.25f}; + t.computeNormals(); + obj->insertTriangle(t); // 0 + t.vertex(0) = {0.10f, -0.10f, 0.20f}; + t.vertex(1) = {0.10f, 0.10f, 0.20f}; + t.vertex(2) = {-0.20f, 0.10f, 0.25f}; + t.computeNormals(); + obj->insertTriangle(t); // 1 + + t.vertex(0) = {0.10f, -0.10f, 0.05f}; + t.vertex(1) = {0.10f, 0.10f, 0.20f}; + t.vertex(2) = {0.10f, -0.10f, 0.20f}; + t.computeNormals(); + obj->insertTriangle(t); // 2 + t.vertex(0) = {0.10f, -0.10f, 0.05f}; + t.vertex(1) = {0.10f, 0.10f, 0.05f}; + t.vertex(2) = {0.10f, 0.10f, 0.20f}; + t.computeNormals(); + obj->insertTriangle(t); // 3 + + t.vertex(0) = {-0.20f, -0.10f, 0.05f}; + t.vertex(1) = {-0.20f, -0.10f, 0.25f}; + t.vertex(2) = {-0.20f, 0.10f, 0.25f}; + t.computeNormals(); + obj->insertTriangle(t); // 2b + t.vertex(0) = {-0.20f, -0.10f, 0.05f}; + t.vertex(1) = {-0.20f, 0.10f, 0.25f}; + t.vertex(2) = {-0.20f, 0.10f, 0.05f}; + t.computeNormals(); + obj->insertTriangle(t); // 3b + + t.vertex(0) = {0.10f, -0.10f, 0.20f}; + t.vertex(1) = {-0.20f, -0.10f, 0.25f}; + t.vertex(2) = {-0.20f, -0.10f, 0.05f}; + t.computeNormals(); + obj->insertTriangle(t); // 4 + + t.vertex(0) = {0.10f, -0.10f, 0.20f}; + t.vertex(1) = {-0.20f, -0.10f, 0.05f}; + t.vertex(2) = {0.10f, -0.10f, 0.05f}; + t.computeNormals(); + obj->insertTriangle(t); // 5 + + t.vertex(0) = {0.10f, 0.10f, 0.20f}; + t.vertex(1) = {-0.20f, 0.10f, 0.05f}; + t.vertex(2) = {-0.20f, 0.10f, 0.25f}; + t.computeNormals(); + obj->insertTriangle(t); // 6 + + t.vertex(0) = {0.10f, 0.10f, 0.20f}; + t.vertex(1) = {0.10f, 0.10f, 0.05f}; + t.vertex(2) = {-0.20f, 0.10f, 0.05f}; + t.computeNormals(); + obj->insertTriangle(t); // 7 + + t.setColor(mrpt::img::TColorf(0.05f, 0.05f, 0.05f, 1)); + + t.vertex(0) = {0.00f, 0.11f, 0.00f}; + t.vertex(1) = {0.00f, 0.11f, 0.10f}; + t.vertex(2) = {0.05f, 0.11f, 0.05f}; + t.computeNormals(); + obj->insertTriangle(t); // 8 + t.vertex(0) = {0.00f, 0.11f, 0.00f}; + t.vertex(1) = {0.00f, 0.11f, 0.10f}; + t.vertex(2) = {-0.05f, 0.11f, 0.05f}; + t.computeNormals(); + obj->insertTriangle(t); // 9 + + t.vertex(0) = {0.00f, -0.11f, 0.00f}; + t.vertex(1) = {0.00f, -0.11f, 0.10f}; + t.vertex(2) = {0.05f, -0.11f, 0.05f}; + t.computeNormals(); + obj->insertTriangle(t); // 10 + t.vertex(0) = {0.00f, -0.11f, 0.00f}; + t.vertex(1) = {0.00f, -0.11f, 0.10f}; + t.vertex(2) = {-0.05f, -0.11f, 0.05f}; + t.computeNormals(); + obj->insertTriangle(t); // 11 + + ret->insert(obj); + + return ret; +} + +/*--------------------------------------------------------------- + CornerXYZ + ---------------------------------------------------------------*/ +CSetOfObjects::Ptr stock_objects::CornerXYZ(float scale) +{ + CSetOfObjects::Ptr ret = std::make_shared(); + + CArrow::Ptr obj = + CArrow::Create(0, 0, 0, scale, 0, 0, 0.25f * scale, 0.02f * scale, 0.05f * scale); + + obj->setColor(1, 0, 0); + ret->insert(obj); + + obj = CArrow::Create(0, 0, 0, 0, scale, 0, 0.25f * scale, 0.02f * scale, 0.05f * scale); + obj->setColor(0, 1, 0); + + ret->insert(obj); + + obj = CArrow::Create(0, 0, 0, 0, 0, scale, 0.25f * scale, 0.02f * scale, 0.05f * scale); + obj->setColor(0, 0, 1); + + ret->insert(obj); + + return ret; +} + +/*--------------------------------------------------------------- + RobotRhodon + ---------------------------------------------------------------*/ +CSetOfObjects::Ptr stock_objects::RobotRhodon() +{ + CSetOfObjects::Ptr ret = std::make_shared(); + float height = 0; + + vector level1; + level1.emplace_back(0.31, 0); + level1.emplace_back(0.22, 0.24); + level1.emplace_back(-0.22, 0.24); + level1.emplace_back(-0.31, 0); + level1.emplace_back(-0.22, -0.24); + level1.emplace_back(0.22, -0.24); + + CPolyhedron::Ptr obj1 = viz::CPolyhedron::CreateCustomPrism(level1, 0.38); + obj1->setLocation(0, 0, height); + height += 0.38f; + obj1->setColor(0.6f, 0.6f, 0.6f); + ret->insert(obj1); + + vector level2; + level2.emplace_back(0.16, 0.21); + level2.emplace_back(-0.16, 0.21); + level2.emplace_back(-0.16, -0.21); + level2.emplace_back(0.16, -0.21); + + CPolyhedron::Ptr obj2 = viz::CPolyhedron::CreateCustomPrism(level2, 0.35); + obj2->setLocation(0, 0, height); + height += 0.35f; + obj2->setColor(0.2f, 0.2f, 0.2f); + ret->insert(obj2); + + vector level3; + level3.emplace_back(-0.12, 0.12); + level3.emplace_back(-0.16, 0.12); + level3.emplace_back(-0.16, -0.12); + level3.emplace_back(-0.12, -0.12); + + CPolyhedron::Ptr obj3 = viz::CPolyhedron::CreateCustomPrism(level3, 1); + obj3->setLocation(0, 0, height); + // height+=1; + obj3->setColor(0.6f, 0.6f, 0.6f); + ret->insert(obj3); + + viz::CCylinder::Ptr obj4 = std::make_shared(0.05f, 0.05f, 0.4f, 20); + obj4->setLocation(0, 0, 0.73); + obj4->setColor(0.0f, 0.0f, 0.9f); + ret->insert(obj4); + + viz::CCylinder::Ptr obj5 = std::make_shared(0.05f, 0.05f, 0.4f, 20); + obj5->setPose(CPose3D(0.32, 0, 0.89, 0, -1, 0)); + obj5->setColor(0.0f, 0.0f, 0.9f); + ret->insert(obj5); + + return ret; +} + +/*--------------------------------------------------------------- + RobotGiraff + ---------------------------------------------------------------*/ +CSetOfObjects::Ptr stock_objects::RobotGiraff() +{ + CSetOfObjects::Ptr ret = std::make_shared(); + float height = 0; + + // Base + vector level1; + level1.emplace_back(0.31, 0); + level1.emplace_back(0.22, 0.24); + level1.emplace_back(-0.22, 0.24); + level1.emplace_back(-0.31, 0); + level1.emplace_back(-0.22, -0.24); + level1.emplace_back(0.22, -0.24); + + CPolyhedron::Ptr obj1 = viz::CPolyhedron::CreateCustomPrism(level1, 0.23); + obj1->setLocation(0, 0, height); + height += 0.23f; + obj1->setColor(1.0f, 0.6f, 0.0f); + ret->insert(obj1); + + // Electronic's cage + vector level2; + level2.emplace_back(0.13, 0.1); + level2.emplace_back(-0.13, 0.1); + level2.emplace_back(-0.13, -0.1); + level2.emplace_back(0.13, -0.1); + + CPolyhedron::Ptr obj2 = viz::CPolyhedron::CreateCustomPrism(level2, 0.45); + obj2->setLocation(0, 0, height); + height += 0.45f; + obj2->setColor(1.0f, 0.6f, 0.2f); + ret->insert(obj2); + + // Neck + vector level3; + level3.emplace_back(0.03, 0.03); + level3.emplace_back(-0.03, 0.03); + level3.emplace_back(-0.03, -0.03); + level3.emplace_back(0.03, -0.03); + + CPolyhedron::Ptr obj3 = viz::CPolyhedron::CreateCustomPrism(level3, 0.55); + obj3->setLocation(0, 0, height); + height += 0.55f; + obj3->setColor(0.6f, 0.6f, 0.6f); + ret->insert(obj3); + + // Screen + vector level4; + level4.emplace_back(0.03, 0.11); + level4.emplace_back(-0.03, 0.11); + level4.emplace_back(-0.03, -0.11); + level4.emplace_back(0.03, -0.11); + + CPolyhedron::Ptr obj4 = viz::CPolyhedron::CreateCustomPrism(level4, 0.4); + obj4->setLocation(0, 0, height); + obj4->setColor(1.0f, 0.6f, 0.0f); + ret->insert(obj4); + + return ret; +} + +CSetOfObjects::Ptr stock_objects::CornerXYZEye() +{ + CSetOfObjects::Ptr ret = std::make_shared(); + CPose3D rotation; + + CArrow::Ptr obj = CArrow::Create(0, 0, 0, 1.0, 0, 0, 0.25f, 0.02f, 0.05f); + + obj->setColor(1, 0, 0); + + ret->insert(obj); + + obj = CArrow::Create(0, 0, 0, 0, 1.0, 0, 0.25f, 0.02f, 0.05f); + obj->setColor(0, 1, 0); + + ret->insert(obj); + + obj = CArrow::Create(0, 0, -1.0, 0, 0, 0, 0.25f, 0.02f, 0.05f); + obj->setColor(0, 0, 1); + + ret->insert(obj); + + return ret; +} + +/*--------------------------------------------------------------- + BumblebeeCamera + ---------------------------------------------------------------*/ +CSetOfObjects::Ptr stock_objects::BumblebeeCamera() +{ + CSetOfObjects::Ptr camera = std::make_shared(); + + CPolyhedron::Ptr rect = viz::CPolyhedron::CreateCubicPrism(-0.02, 0.14, -0.02, 0.02, 0, -0.04); + rect->setColor(1.0f, 0.8f, 0.0f); + + camera->insert(rect); + + CCylinder::Ptr lCam = std::make_shared(0.01f, 0.01f, 0.003f, 10); + lCam->setColor(1, 0, 0); + + CCylinder::Ptr rCam = std::make_shared(0.01f, 0.01f, 0.003f, 10); + rCam->setPose(CPose3D(0.12, 0, 0)); + rCam->setColor(0, 0, 0); + + camera->insert(lCam); + camera->insert(rCam); + + return camera; +} + +CSetOfObjects::Ptr stock_objects::CornerXYZSimple(float scale, float lineWidth) +{ + CSetOfObjects::Ptr ret = std::make_shared(); + + // Using OpenGL shaders, it's more complicated to set line widths. + // So, let's use cylinders of a diameter proportional to "scale": + const float R = scale * 0.01f; + const int nSlices = 6; + + { // X: + auto lin = CCylinder::Create(R, R, scale, nSlices); + lin->setColor(1, 0, 0); + lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 90 0]")); + ret->insert(lin); + } + { // Y: + auto lin = CCylinder::Create(R, R, scale, nSlices); + lin->setColor(0, 1, 0); + lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 0 -90]")); + ret->insert(lin); + } + { // Z: + auto lin = CCylinder::Create(R, R, scale, nSlices); + lin->setColor(0, 0, 1); + ret->insert(lin); + } + return ret; +} + +CSetOfObjects::Ptr stock_objects::CornerXYSimple(float scale, float lineWidth) +{ + CSetOfObjects::Ptr ret = std::make_shared(); + + // Using OpenGL shaders, it's more complicated to set line widths. + // So, let's use cylinders of a diameter proportional to "scale": + const float R = scale * 0.01f; + const int nSlices = 6; + + { // X: + auto lin = CCylinder::Create(R, R, scale, nSlices); + lin->setColor(1, 0, 0); + lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 90 0]")); + ret->insert(lin); + } + { // Y: + auto lin = CCylinder::Create(R, R, scale, nSlices); + lin->setColor(0, 1, 0); + lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 0 -90]")); + ret->insert(lin); + } + return ret; +} + +CSetOfObjects::Ptr stock_objects::Hokuyo_URG() +{ + CSetOfObjects::Ptr ret = std::make_shared(); + + { + CBox::Ptr base = + std::make_shared(TPoint3D(-0.025, -0.025, -0.0575), TPoint3D(0.025, 0.025, -0.0185)); + base->setColor(0.7f, 0.7f, 0.7f); + ret->insert(base); + } + { + CCylinder::Ptr cyl1 = std::make_shared(0.02f, 0.02f, 0.01f); + cyl1->setColor(0, 0, 0); + cyl1->setLocation(0, 0, -0.014); + ret->insert(cyl1); + } + { + CCylinder::Ptr cyl2 = std::make_shared(0.02f, 0.0175f, 0.01f); + cyl2->setColor(0, 0, 0); + cyl2->setLocation(0, 0, -0.004); + ret->insert(cyl2); + } + { + CCylinder::Ptr cyl3 = std::make_shared(0.0175f, 0.0175f, 0.01f); + cyl3->setColor(0, 0, 0); + cyl3->setLocation(0, 0, 0.004); + ret->insert(cyl3); + } + + return ret; +} + +CSetOfObjects::Ptr stock_objects::Hokuyo_UTM() +{ + CSetOfObjects::Ptr ret = std::make_shared(); + + { + CBox::Ptr base = + std::make_shared(TPoint3D(-0.03, -0.03, -0.055), TPoint3D(0.03, 0.03, -0.014)); + base->setColor(0, 0, 0); + ret->insert(base); + } + { + CCylinder::Ptr cyl1 = std::make_shared(0.028f, 0.024f, 0.028f); + cyl1->setColor(0, 0, 0); + cyl1->setPose(CPose3D(0, 0, -0.014)); + ret->insert(cyl1); + } + { + CCylinder::Ptr cyl2 = std::make_shared(0.028f, 0.028f, 0.01f); + cyl2->setColor(1.0f, 69.0f / 255.0f, 0); + cyl2->setLocation(0, 0, 0.014); + ret->insert(cyl2); + } + { + CCylinder::Ptr cyl3 = std::make_shared(0.028f, 0.028f, 0.01f); + cyl3->setColor(0, 0, 0); + cyl3->setLocation(0, 0, 0.024); + ret->insert(cyl3); + } + + return ret; +} + +CSetOfObjects::Ptr stock_objects::Househam_Sprayer() +{ + CSetOfObjects::Ptr ret = std::make_shared(); + + { + CBox::Ptr cabin = + std::make_shared(TPoint3D(0.878, 0.723, -0.12), TPoint3D(-0.258, -0.723, -1.690)); + cabin->setColor(0.7f, 0.7f, 0.7f); + ret->insert(cabin); + } + { + CBox::Ptr back = + std::make_shared(TPoint3D(-0.258, 0.723, -0.72), TPoint3D(-5.938, -0.723, -1.690)); + back->setColor(1, 1, 1); + ret->insert(back); + } + { + CBox::Ptr boomAxis = + std::make_shared(TPoint3D(-5.938, 0.723, -1.0), TPoint3D(-6.189, -0.723, -1.690)); + boomAxis->setColor(0, 0, 0); + ret->insert(boomAxis); + } + { + CBox::Ptr boom1 = + std::make_shared(TPoint3D(-5.938, 0.723, -1.0), TPoint3D(-6.189, 11.277, -1.620)); + boom1->setColor(0, 1, 0); + ret->insert(boom1); + } + { + CBox::Ptr boom2 = + std::make_shared(TPoint3D(-5.938, -0.723, -1.0), TPoint3D(-6.189, -11.277, -1.620)); + boom2->setColor(0, 1, 0); + ret->insert(boom2); + } + { + CCylinder::Ptr cyl1 = std::make_shared(0.716f, 0.716f, 0.387f, 30); + cyl1->setColor(0, 0, 0); + cyl1->setPose(CPose3D(-0.710, 0.923, -2.480, 0, 0, 90.0_deg)); + ret->insert(cyl1); + } + { + CCylinder::Ptr cyl2 = std::make_shared(0.716f, 0.716f, 0.387f, 30); + cyl2->setColor(0, 0, 0); + cyl2->setPose(CPose3D(-3.937, 0.923, -2.480, 0, 0, 90.0_deg)); + ret->insert(cyl2); + } + { + CCylinder::Ptr cyl1 = std::make_shared(0.716f, 0.716f, 0.387f, 30); + cyl1->setColor(0, 0, 0); + cyl1->setPose(CPose3D(-0.710, -0.423, -2.480, 0, 0, 90.0_deg)); + ret->insert(cyl1); + } + { + CCylinder::Ptr cyl2 = std::make_shared(0.716f, 0.716f, 0.387f, 30); + cyl2->setColor(0, 0, 0); + cyl2->setPose(CPose3D(-3.937, -0.423, -2.480, 0, 0, 90.0_deg)); + ret->insert(cyl2); + } + return ret; +} diff --git a/libs/viz/src/TLightParameters.cpp b/libs/viz/src/TLightParameters.cpp new file mode 100644 index 0000000000..84b730a7e5 --- /dev/null +++ b/libs/viz/src/TLightParameters.cpp @@ -0,0 +1,73 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace std; + +void TLightParameters::writeToStream(mrpt::serialization::CArchive& out) const +{ + const uint8_t version = 3; + out << version; + + out << diffuse << ambient << specular << direction << color; + out << shadow_bias << shadow_bias_cam2frag << shadow_bias_normal; // v2 + out << eyeDistance2lightShadowExtension << minimum_shadow_map_extension_ratio; // v3 +} + +void TLightParameters::readFromStream(mrpt::serialization::CArchive& in) +{ + uint8_t version; + in >> version; + + switch (version) + { + case 0: + { + mrpt::img::TColorf diffuseCol, ambientCol, specularCol; + in >> diffuseCol >> ambientCol >> specularCol >> direction; + ambient = ambientCol.R; + specular = specularCol.R; + diffuse = 1.0f; + color = diffuseCol; + } + break; + case 1: + case 2: + case 3: + in >> diffuse >> ambient >> specular >> direction >> color; + if (version >= 2) in >> shadow_bias >> shadow_bias_cam2frag >> shadow_bias_normal; + if (version >= 3) + in >> eyeDistance2lightShadowExtension >> minimum_shadow_map_extension_ratio; + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +namespace mrpt::viz +{ +mrpt::serialization::CArchive& operator>>( + mrpt::serialization::CArchive& in, mrpt::viz::TLightParameters& o) +{ + o.readFromStream(in); + return in; +} +mrpt::serialization::CArchive& operator<<( + mrpt::serialization::CArchive& out, const mrpt::viz::TLightParameters& o) +{ + o.writeToStream(out); + return out; +} +} // namespace mrpt::viz diff --git a/libs/viz/src/TTriangle.cpp b/libs/viz/src/TTriangle.cpp new file mode 100644 index 0000000000..9e7da01138 --- /dev/null +++ b/libs/viz/src/TTriangle.cpp @@ -0,0 +1,49 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +using namespace mrpt::viz; + +// packet size= 3 vertices, each: +// XYZ (float) + normal (XYZ float) + RGBA (u8) + UV (float) +static_assert(sizeof(TTriangle) == (sizeof(float) * (3 + 3 + 2) + 4) * 3); + +void TTriangle::computeNormals() +{ + const float ax = x(1) - x(0); + const float ay = y(1) - y(0); + const float az = z(1) - z(0); + const float bx = x(2) - x(0); + const float by = y(2) - y(0); + const float bz = z(2) - z(0); + + const mrpt::math::TVector3Df no = {ay * bz - az * by, -ax * bz + az * bx, ax * by - ay * bx}; + for (auto& v : vertices) v.normal = no; +} + +void TTriangle::writeTo(mrpt::serialization::CArchive& o) const +{ + for (const auto& p : vertices) + { + const auto& pp = p.xyzrgba; + o << pp.pt << pp.r << pp.g << pp.b << pp.a << p.normal; + } +} +void TTriangle::readFrom(mrpt::serialization::CArchive& in) +{ + for (auto& p : vertices) + { + auto& pp = p.xyzrgba; + in >> pp.pt >> pp.r >> pp.g >> pp.b >> pp.a >> p.normal; + } +} diff --git a/libs/viz/src/Viewport.cpp b/libs/viz/src/Viewport.cpp new file mode 100644 index 0000000000..aee29245cc --- /dev/null +++ b/libs/viz/src/Viewport.cpp @@ -0,0 +1,512 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include // First! to avoid conflicts with X.h +// +#include +#include +#include // crossProduct3D() +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::poses; +using namespace mrpt::viz; +using namespace mrpt::math; +using namespace mrpt::serialization::metaprogramming; +using namespace std; + +IMPLEMENTS_SERIALIZABLE(Viewport, CSerializable, mrpt::viz) + +/*-------------------------------------------------------------- + + IMPLEMENTATION OF Viewport + + ---------------------------------------------------------------*/ + +/*-------------------------------------------------------------- + Constructor + ---------------------------------------------------------------*/ +Viewport::Viewport(Scene* parent, const string& name) : m_parent(parent), m_name(name) {} + +Viewport::~Viewport() { clear(); } + +void Viewport::setCloneView(const string& clonedViewport) +{ + clear(); + m_isCloned = true; + m_clonedViewport = clonedViewport; +} + +void Viewport::setViewportPosition( + const double x, const double y, const double width, const double height) +{ + MRPT_START + + m_view_x = x; + m_view_y = y; + m_view_width = width; + m_view_height = height; + + MRPT_END +} + +/*-------------------------------------------------------------- + getViewportPosition + ---------------------------------------------------------------*/ +void Viewport::getViewportPosition(double& x, double& y, double& width, double& height) +{ + x = m_view_x; + y = m_view_y; + width = m_view_width; + height = m_view_height; +} + +/*-------------------------------------------------------------- + clear + ---------------------------------------------------------------*/ +void Viewport::clear() { m_objects.clear(); } +/*-------------------------------------------------------------- + insert + ---------------------------------------------------------------*/ +void Viewport::insert(const CVisualObject::Ptr& newObject) { m_objects.push_back(newObject); } + +uint8_t Viewport::serializeGetVersion() const { return 10; } +void Viewport::serializeTo(mrpt::serialization::CArchive& out) const +{ + // Save data: + out << m_camera << m_isCloned << m_isClonedCamera << m_clonedViewport << m_name << m_isTransparent + << m_borderWidth << m_view_x << m_view_y << m_view_width << m_view_height; + + // Added in v1: + out << m_background_color.R << m_background_color.G << m_background_color.B + << m_background_color.A; + + // Save objects: + uint32_t n; + n = (uint32_t)m_objects.size(); + out << n; + for (const auto& m_object : m_objects) out << *m_object; + + // Added in v2: Global OpenGL settings: + out << m_OpenGL_enablePolygonNicest; + + // Added in v3: Lights + out << m_light; + + // Added in v4: text messages: + std::shared_lock lckRead2DTexts(m_2D_texts.mtx.data); + + out.WriteAs(m_2D_texts.messages.size()); + for (auto& kv : m_2D_texts.messages) + { + out << kv.first; // id + out << kv.second.x << kv.second.y << kv.second.text; + + const auto& fp = kv.second; + + out << fp.vfont_name << fp.vfont_scale << fp.color << fp.draw_shadow << fp.shadow_color + << fp.vfont_spacing << fp.vfont_kerning; + out.WriteAs(static_cast(fp.vfont_style)); + } + lckRead2DTexts.unlock(); + + // Added in v5: image mode + out.WriteAs(m_imageViewPlane); + if (m_imageViewPlane) out << *m_imageViewPlane; + + // Added in v6: + out << m_clonedCameraViewport; + + // Added in v8: + out << m_shadowsEnabled << m_ShadowMapSizeX << m_ShadowMapSizeY; + + // Added in v9: + out << m_clip_max << m_clip_min << m_lightShadowClipMin << m_lightShadowClipMax; + + // v10: + out << m_isViewportVisible; +} + +void Viewport::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + case 1: + case 2: + case 3: + case 4: + case 5: + case 6: + case 7: + case 8: + case 9: + case 10: + { + // Load data: + in >> m_camera >> m_isCloned >> m_isClonedCamera >> m_clonedViewport >> m_name >> + m_isTransparent >> m_borderWidth >> m_view_x >> m_view_y >> m_view_width >> m_view_height; + + // in v1: + if (version >= 1) + { + if (version < 7) + { + // field removed in v7: + bool was_m_custom_backgb_color; + in >> was_m_custom_backgb_color; + } + + in >> m_background_color.R >> m_background_color.G >> m_background_color.B >> + m_background_color.A; + } + + // Load objects: + uint32_t n; + in >> n; + clear(); + m_objects.resize(n); + + for_each(m_objects.begin(), m_objects.end(), ObjectReadFromStream(&in)); + + // Added in v2: Global OpenGL settings: + if (version >= 2) + { + in >> m_OpenGL_enablePolygonNicest; + } + else + { + // Defaults + } + + // Added in v3: Lights + if (version >= 3) + in >> m_light; + else + { + // Default: + m_light = TLightParameters(); + } + + // v4: text: + m_2D_texts.messages.clear(); + uint32_t nTexts = 0; + if (version >= 4) nTexts = in.ReadAs(); + + for (uint32_t i = 0; i < nTexts; i++) + { + const auto id = in.ReadAs(); + double x, y; + std::string text; + in >> x >> y >> text; + + TFontParams fp; + + in >> fp.vfont_name >> fp.vfont_scale >> fp.color >> fp.draw_shadow >> fp.shadow_color >> + fp.vfont_spacing >> fp.vfont_kerning; + fp.vfont_style = static_cast(in.ReadAs()); + + this->addTextMessage(x, y, text, id, fp); + } + + // Added in v5: image mode + if (in.ReadAs()) + { + in >> m_imageViewPlane; + } + else + { + m_imageViewPlane.reset(); + } + + if (version >= 6) + in >> m_clonedCameraViewport; + else + m_clonedCameraViewport.clear(); + + if (version >= 8) + { + in >> m_shadowsEnabled >> m_ShadowMapSizeX >> m_ShadowMapSizeY; + } + else + { + m_shadowsEnabled = false; + } + + if (version >= 9) + { + in >> m_clip_max >> m_clip_min >> m_lightShadowClipMin >> m_lightShadowClipMax; + } + + if (version >= 10) + { + in >> m_isViewportVisible; + } + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +/*--------------------------------------------------------------- + getByName + ---------------------------------------------------------------*/ +CVisualObject::Ptr Viewport::getByName(const string& str) +{ + for (auto& m_object : m_objects) + { + if (m_object->getName() == str) + return m_object; + else if (m_object->GetRuntimeClass() == CLASS_ID_NAMESPACE(CSetOfObjects, mrpt::viz)) + { + if (CVisualObject::Ptr ret = + std::dynamic_pointer_cast(m_object)->getByName(str); + ret) + return ret; + } + } + return {}; +} + +void Viewport::dumpListOfObjects(std::vector& lst) const +{ + for (auto& obj : m_objects) + { + // Single obj: + string s(obj->GetRuntimeClass()->className); + if (!obj->getName().empty()) s += string(" (") + obj->getName() + string(")"); + lst.emplace_back(s); + + if (obj->GetRuntimeClass() == CLASS_ID_NAMESPACE(CSetOfObjects, mrpt::viz)) + { + std::vector auxLst; + + dynamic_cast(obj.get())->dumpListOfObjects(auxLst); + + for (const auto& i : auxLst) lst.emplace_back(string(" ") + i); + } + } +} + +mrpt::containers::yaml Viewport::asYAML() const +{ + mrpt::containers::yaml d = mrpt::containers::yaml::Sequence(); + + d.asSequence().resize(m_objects.size()); + + for (uint32_t i = 0; i < m_objects.size(); i++) + { + const auto obj = m_objects.at(i); + mrpt::containers::yaml de = mrpt::containers::yaml::Map(); + + // class-specific properties: + obj->toYAMLMap(de); + + de["index"] = i; // type for "i" must be a stdint type + if (!obj) + { + de["class"] = "nullptr"; + continue; + } + de["class"] = obj->GetRuntimeClass()->className; + + // Single obj: + if (obj->GetRuntimeClass() == CLASS_ID_NAMESPACE(CSetOfObjects, mrpt::viz)) + { + de["obj_children"] = dynamic_cast(obj.get())->asYAML(); + } + d.asSequence().at(i) = std::move(de); + } + return d; +} + +/*-------------------------------------------------------------- + removeObject + ---------------------------------------------------------------*/ +void Viewport::removeObject(const CVisualObject::Ptr& obj) +{ + for (auto it = m_objects.begin(); it != m_objects.end(); ++it) + if (*it == obj) + { + m_objects.erase(it); + return; + } + else if ((*it)->GetRuntimeClass() == CLASS_ID_NAMESPACE(CSetOfObjects, mrpt::viz)) + dynamic_cast(it->get())->removeObject(obj); +} + +void Viewport::setViewportClipDistances(const float clip_min, const float clip_max) +{ + ASSERT_GT_(clip_max, clip_min); + + m_clip_min = clip_min; + m_clip_max = clip_max; +} + +void Viewport::getViewportClipDistances(float& clip_min, float& clip_max) const +{ + clip_min = m_clip_min; + clip_max = m_clip_max; +} + +void Viewport::setLightShadowClipDistances(const float clip_min, const float clip_max) +{ + ASSERT_GT_(clip_max, clip_min); + + m_lightShadowClipMin = clip_min; + m_lightShadowClipMax = clip_max; +} + +void Viewport::getLightShadowClipDistances(float& clip_min, float& clip_max) const +{ + clip_min = m_lightShadowClipMin; + clip_max = m_lightShadowClipMax; +} + +void Viewport::setCurrentCameraFromPose(mrpt::poses::CPose3D& p) +{ + m_camera.set6DOFMode(true); + m_camera.setPose(p); +} + +/** Resets the viewport to a normal 3D viewport \sa setCloneView, setImageView + */ +void Viewport::setNormalMode() +{ + // If this was an image-mode viewport, remove the quad object to disable it. + m_imageViewPlane.reset(); + + m_isCloned = false; + m_isClonedCamera = false; +} + +void Viewport::setImageView(const mrpt::img::CImage& img, bool transparentBackground) +{ + internal_enableImageView(transparentBackground); + m_imageViewPlane->assignImage(img); +} +void Viewport::setImageView(mrpt::img::CImage&& img, bool transparentBackground) +{ + internal_enableImageView(transparentBackground); + m_imageViewPlane->assignImage(img); +} + +void Viewport::internal_enableImageView(bool transparentBackground) +{ + // If this is the first time, we have to create the quad object: + if (!m_imageViewPlane) + { + m_imageViewPlane = mrpt::viz::CTexturedPlane::Create(); + // Flip vertically: + m_imageViewPlane->setPlaneCorners(-1, 1, 1, -1); + } + setTransparent(transparentBackground); +} + +/** Evaluates the bounding box of this object (including possible children) in + * the coordinate frame of the object parent. */ +auto Viewport::getBoundingBox() const -> mrpt::math::TBoundingBox +{ + mrpt::math::TBoundingBox bb; + bool first = true; + + for (const auto& o : m_objects) + { + if (first) + { + bb = o->getBoundingBox(); + first = false; + } + else + bb = bb.unionWith(o->getBoundingBox()); + } + + return bb; +} + +void Viewport::setCloneCamera(bool enable) +{ + m_isClonedCamera = enable; + if (!enable) + { + m_clonedCameraViewport.clear(); + } + else + { + ASSERTMSG_( + !m_clonedViewport.empty(), + "Error: cannot setCloneCamera(true) on a viewport before calling " + "setCloneView()"); + + m_clonedCameraViewport = m_clonedViewport; + } +} + +const CCamera* Viewport::internalResolveActiveCamera(const CCamera* forceThisCamera) const +{ + // Prepare camera (projection matrix): + Viewport* viewForGetCamera = nullptr; + + if (!m_clonedCameraViewport.empty()) + { + const auto view = m_parent->getViewport(m_clonedCameraViewport); + if (!view) + THROW_EXCEPTION_FMT( + "Cloned viewport '%s' not found in parent Scene", m_clonedViewport.c_str()); + + viewForGetCamera = m_isClonedCamera ? view.get() : const_cast(this); + } + else + { // Normal case: render our own objects: + viewForGetCamera = const_cast(this); + } + + // Get camera: + // 1st: if there is a CCamera in the scene (nullptr if no camera found): + const auto camPtr = viewForGetCamera->getByClass(); + const auto* myCamera = camPtr ? camPtr.get() : nullptr; + + // 2nd: the internal camera of all viewports: + if (!myCamera) myCamera = &viewForGetCamera->m_camera; + + // forced cam? + if (forceThisCamera) myCamera = forceThisCamera; + + return myCamera; +} + +void Viewport::enableShadowCasting( + bool enabled, unsigned int SHADOW_MAP_SIZE_X, unsigned int SHADOW_MAP_SIZE_Y) +{ + m_shadowsEnabled = enabled; + if (SHADOW_MAP_SIZE_X) m_ShadowMapSizeX = SHADOW_MAP_SIZE_X; + if (SHADOW_MAP_SIZE_Y) m_ShadowMapSizeY = SHADOW_MAP_SIZE_Y; +} + +Viewport::Ptr& mrpt::viz::operator<<(Viewport::Ptr& s, const CVisualObject::Ptr& r) +{ + s->insert(r); + return s; +} + +Viewport::Ptr& mrpt::viz::operator<<(Viewport::Ptr& s, const std::vector& v) +{ + for (const auto& it : v) s->insert(it); + return s; +} diff --git a/libs/viz/src/Visualizable.cpp b/libs/viz/src/Visualizable.cpp new file mode 100644 index 0000000000..383989a7b3 --- /dev/null +++ b/libs/viz/src/Visualizable.cpp @@ -0,0 +1,22 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include + +// Note: This method cannot be defined into the .h to avoid including the +// full CSetOfObjects.h header. +mrpt::viz::CSetOfObjects::Ptr mrpt::viz::Visualizable::getVisualization() const +{ + auto o = mrpt::viz::CSetOfObjects::Create(); + getVisualizationInto(*o); + return o; +} diff --git a/libs/viz/src/pose_pdfs.cpp b/libs/viz/src/pose_pdfs.cpp new file mode 100644 index 0000000000..c21ac9297c --- /dev/null +++ b/libs/viz/src/pose_pdfs.cpp @@ -0,0 +1,309 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::math; +using namespace mrpt::viz; +using namespace mrpt::poses; + +const double POSE_TAIL_LENGTH = 0.1; +const double POSE_TAIL_WIDTH = 3.0; +const float POSE_POINT_SIZE = 4.0f; +const float POSE_AXIS_SCALE = 0.1f; + +#define POSE_COLOR 0, 0, 1 +#define POINT_COLOR 1, 0, 0 + +/** Returns a representation of a the PDF - this is just an auxiliary function, + * it's more natural to call + * mrpt::poses::CPosePDF::getAs3DObject */ +CSetOfObjects::Ptr CSetOfObjects::posePDF2opengl(const CPosePDF& o) +{ + auto outObj = CSetOfObjects::Create(); + + if (IS_CLASS(o, CPosePDFSOG)) + { + const auto* p = dynamic_cast(&o); + ASSERT_(p != nullptr); + + viz::CSetOfLines::Ptr lins = std::make_shared(); + lins->setColor(0.0f, 0.0f, 1.0f, 0.6f); + lins->setLineWidth(POSE_TAIL_WIDTH); + + for (const auto& it : *p) + { + auto ellip = mrpt::viz::CEllipsoid2D::Create(); + + ellip->setPose(CPose3D(it.mean.x(), it.mean.y(), 0)); + ellip->setCovMatrix(it.cov.blockCopy<2, 2>()); + ellip->setColor(POSE_COLOR, 0.6f); + ellip->setQuantiles(3); + ellip->enableDrawSolid3D(false); + + outObj->insert(ellip); + + lins->appendLine( + it.mean.x(), it.mean.y(), 0, it.mean.x() + POSE_TAIL_LENGTH * cos(it.mean.phi()), + it.mean.y() + POSE_TAIL_LENGTH * sin(it.mean.phi()), 0); + } + outObj->insert(lins); + } + else if (IS_CLASS(o, CPosePDFGaussian)) + { + const auto* p = dynamic_cast(&o); + ASSERT_(p != nullptr); + + viz::CSetOfLines::Ptr lins = std::make_shared(); + lins->setColor(POSE_COLOR, 0.6f); + lins->setLineWidth(POSE_TAIL_WIDTH); + + auto ellip = mrpt::viz::CEllipsoid2D::Create(); + + ellip->setPose(CPose3D(p->mean.x(), p->mean.y(), 0)); + ellip->setCovMatrix(p->cov.blockCopy<2, 2>()); + ellip->setColor(POSE_COLOR, 0.6f); + + ellip->setQuantiles(3); + ellip->enableDrawSolid3D(false); + + outObj->insert(ellip); + + lins->appendLine( + p->mean.x(), p->mean.y(), 0, p->mean.x() + POSE_TAIL_LENGTH * cos(p->mean.phi()), + p->mean.y() + POSE_TAIL_LENGTH * sin(p->mean.phi()), 0); + + outObj->insert(lins); + } + else if (IS_CLASS(o, CPosePDFParticles)) + { + const auto* p = dynamic_cast(&o); + ASSERT_(p != nullptr); + + auto pnts = viz::CPointCloud::Create(); + pnts->setColor(POSE_COLOR, 0.6f); + pnts->setPointSize(POSE_POINT_SIZE); + + auto lins = viz::CSetOfLines::Create(); + lins->setColor(POSE_COLOR, 0.6f); + lins->setLineWidth(POSE_TAIL_WIDTH); + + for (size_t i = 0; i < p->size(); ++i) + { + const auto po = p->m_particles[i].d; + pnts->insertPoint(d2f(po.x), d2f(po.y), 0); + lins->appendLine( + po.x, po.y, 0, po.x + POSE_TAIL_LENGTH * cos(po.phi), + po.y + POSE_TAIL_LENGTH * sin(po.phi), 0); + } + outObj->insert(pnts); + outObj->insert(lins); + } + + return outObj; +} + +/** Returns a representation of a the PDF - this is just an auxiliary function, + * it's more natural to call + * mrpt::poses::CPointPDF::getAs3DObject */ +CSetOfObjects::Ptr CSetOfObjects::posePDF2opengl(const CPointPDF& o) +{ + auto outObj = std::make_shared(); + + if (IS_CLASS(o, CPointPDFSOG)) + { + const auto* p = dynamic_cast(&o); + ASSERT_(p != nullptr); + + // For each gaussian node + for (const auto& it : *p) + { + mrpt::viz::CVisualObject::Ptr obj; + if (it.val.cov(2, 2) == 0) + { + auto ellip = mrpt::viz::CEllipsoid2D::Create(); + ellip->setCovMatrix(it.val.cov.blockCopy<2, 2>()); + ellip->setQuantiles(3); + ellip->enableDrawSolid3D(false); + obj = ellip; + } + else + { + auto ellip = mrpt::viz::CEllipsoid3D::Create(); + ellip->setCovMatrix(it.val.cov); + ellip->setQuantiles(3); + ellip->enableDrawSolid3D(false); + obj = ellip; + } + obj->setPose(it.val.mean); + obj->setColor(POINT_COLOR); + + outObj->insert(obj); + } // end for each gaussian node + } + else if (IS_CLASS(o, CPointPDFGaussian)) + { + const auto* p = dynamic_cast(&o); + ASSERT_(p != nullptr); + + mrpt::viz::CVisualObject::Ptr obj; + if (p->cov(2, 2) == 0) + { + auto ellip = mrpt::viz::CEllipsoid2D::Create(); + ellip->setCovMatrix(p->cov.blockCopy<2, 2>()); + ellip->setQuantiles(3); + ellip->enableDrawSolid3D(false); + obj = ellip; + } + else + { + auto ellip = mrpt::viz::CEllipsoid3D::Create(); + ellip->setCovMatrix(p->cov); + ellip->setQuantiles(3); + ellip->enableDrawSolid3D(false); + obj = ellip; + } + obj->setLocation(p->mean.x(), p->mean.y(), p->mean.z()); + obj->setColor(POINT_COLOR); + outObj->insert(obj); + } + else if (IS_CLASS(o, CPointPDFParticles)) + { + const auto* p = dynamic_cast(&o); + ASSERT_(p != nullptr); + + mrpt::viz::CPointCloud::Ptr obj = mrpt::viz::CPointCloud::Create(); + const size_t N = p->size(); + + obj->resize(N); + obj->setColor(POINT_COLOR); + for (size_t i = 0; i < N; i++) + obj->setPoint(i, p->m_particles[i].d->x, p->m_particles[i].d->y, p->m_particles[i].d->z); + outObj->insert(obj); + } + + return outObj; +} + +/** Returns a representation of a the PDF - this is just an auxiliary function, + * it's more natural to call + * mrpt::poses::CPose3DPDF::getAs3DObject */ +CSetOfObjects::Ptr CSetOfObjects::posePDF2opengl(const CPose3DPDF& o) +{ + CSetOfObjects::Ptr outObj = std::make_shared(); + + if (IS_CLASS(o, CPose3DPDFSOG)) + { + const auto* p = dynamic_cast(&o); + ASSERT_(p != nullptr); + + // For each gaussian node + for (const auto& it : *p) + { + viz::CEllipsoid3D::Ptr obj = std::make_shared(); + + obj->setPose(it.val.mean); + obj->setCovMatrix(it.val.cov.blockCopy<3, 3>()); + + obj->setQuantiles(3); + obj->enableDrawSolid3D(false); + obj->setColor(POSE_COLOR); + + outObj->insert(obj); + + viz::CSetOfObjects::Ptr axes = viz::stock_objects::CornerXYZ(); + axes->setPose(it.val.mean); + axes->setScale(POSE_AXIS_SCALE); + outObj->insert(axes); + } // end for each gaussian node + } + else if (IS_CLASS(o, CPose3DPDFGaussian)) + { + const auto* p = dynamic_cast(&o); + ASSERT_(p != nullptr); + + auto obj = std::make_shared(); + + obj->setPose(p->mean); + obj->setCovMatrix(p->cov.blockCopy<3, 3>()); + + obj->setQuantiles(3); + obj->enableDrawSolid3D(false); + obj->setColor(POSE_COLOR); + + outObj->insert(obj); + + viz::CSetOfObjects::Ptr axes = viz::stock_objects::CornerXYZ(); + axes->setPose(p->mean); + axes->setScale(POSE_AXIS_SCALE); + outObj->insert(axes); + } + else if (IS_CLASS(o, CPose3DPDFParticles)) + { + const auto* p = dynamic_cast(&o); + ASSERT_(p != nullptr); + + for (size_t i = 0; i < p->size(); i++) + { + viz::CSetOfObjects::Ptr axes = viz::stock_objects::CornerXYZSimple(POSE_AXIS_SCALE); + axes->setPose(p->m_particles[i].d); + outObj->insert(axes); + } + } + + return outObj; +} + +/** Returns a representation of a the PDF - this is just an auxiliary function, + * it's more natural to call + * mrpt::poses::CPose3DQuatPDF::getAs3DObject */ +CSetOfObjects::Ptr CSetOfObjects::posePDF2opengl(const CPose3DQuatPDF& o) +{ + CSetOfObjects::Ptr outObj = std::make_shared(); + + if (IS_CLASS(o, CPose3DQuatPDFGaussian)) + { + const auto* p = dynamic_cast(&o); + ASSERT_(p != nullptr); + + auto obj = mrpt::viz::CEllipsoid3D::Create(); + + obj->setPose(CPose3D(p->mean)); + obj->setCovMatrix(p->cov.blockCopy<3, 3>()); + + obj->setQuantiles(3); + obj->enableDrawSolid3D(false); + obj->setColor(POSE_COLOR); + + outObj->insert(obj); + + viz::CSetOfObjects::Ptr axes = viz::stock_objects::CornerXYZ(); + axes->setPose(CPose3D(p->mean)); + axes->setScale(POSE_AXIS_SCALE); + outObj->insert(axes); + } + + return outObj; +} diff --git a/libs/viz/src/registerAllClasses.cpp b/libs/viz/src/registerAllClasses.cpp new file mode 100644 index 0000000000..93936ac0fa --- /dev/null +++ b/libs/viz/src/registerAllClasses.cpp @@ -0,0 +1,127 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header +// +#include + +// My classes: +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// deps: +#include +#include +#include + +MRPT_INITIALIZER(registerAllClasses_mrpt_viz) +{ + using namespace mrpt::viz; + +#if !defined(DISABLE_MRPT_AUTO_CLASS_REGISTRATION) + // Opengl classes: + registerClass(CLASS_ID(Scene)); + mrpt::rtti::registerClassCustomName("mrpt::opengl::COpenGLScene", CLASS_ID(Scene)); + + registerClass(CLASS_ID(Viewport)); + mrpt::rtti::registerClassCustomName("mrpt::opengl::COpenGLViewport", CLASS_ID(Scene)); + + registerClass(CLASS_ID(CArrow)); + registerClass(CLASS_ID(CAssimpModel)); + registerClass(CLASS_ID(CAxis)); + registerClass(CLASS_ID(CBox)); + registerClass(CLASS_ID(CCamera)); + registerClass(CLASS_ID(CColorBar)); + registerClass(CLASS_ID(CCylinder)); + registerClass(CLASS_ID(CDisk)); + registerClass(CLASS_ID(CEllipsoid2D)); + registerClass(CLASS_ID(CEllipsoid3D)); + registerClass(CLASS_ID(CEllipsoidInverseDepth2D)); + registerClass(CLASS_ID(CEllipsoidInverseDepth3D)); + registerClass(CLASS_ID(CEllipsoidRangeBearing2D)); + registerClass(CLASS_ID(CFrustum)); + registerClass(CLASS_ID(CGridPlaneXY)); + registerClass(CLASS_ID(CGridPlaneXZ)); + registerClass(CLASS_ID(CMesh)); + registerClass(CLASS_ID(CMesh3D)); + registerClass(CLASS_ID(CMeshFast)); + registerClass(CLASS_ID(COctoMapVoxels)); + registerClass(CLASS_ID(CPointCloud)); + registerClass(CLASS_ID(CPointCloudColoured)); + registerClass(CLASS_ID(CPolyhedron)); + registerClass(CLASS_ID(CSetOfLines)); + registerClass(CLASS_ID(CSetOfObjects)); + registerClass(CLASS_ID(CSetOfTriangles)); + registerClass(CLASS_ID(CSetOfTexturedTriangles)); + registerClass(CLASS_ID(CSkyBox)); + registerClass(CLASS_ID(CSimpleLine)); + registerClass(CLASS_ID(CSphere)); + registerClass(CLASS_ID(CText)); + registerClass(CLASS_ID(CText3D)); + registerClass(CLASS_ID(CTexturedPlane)); + registerClass(CLASS_ID(CVectorField2D)); + registerClass(CLASS_ID(CVectorField3D)); + +// These ones are in the lib: mrpt-obsmaps +// registerClass( CLASS_ID( CPlanarLaserScan ) ); +// registerClass( CLASS_ID( CAngularObservationMesh ) ); +#endif +} + +void mrpt::viz::registerAllClasses_mrpt_viz() +{ + ::registerAllClasses_mrpt_viz(); + // deps: + mrpt::img::registerAllClasses_mrpt_img(); + mrpt::poses::registerAllClasses_mrpt_poses(); +} diff --git a/libs/viz/src/serializations_unittest.cpp b/libs/viz/src/serializations_unittest.cpp new file mode 100644 index 0000000000..8f59fc32d5 --- /dev/null +++ b/libs/viz/src/serializations_unittest.cpp @@ -0,0 +1,164 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#define MRPT_NO_WARN_BIG_HDR +#include +#include +#include +#include +#include +// +#include +#ifndef MRPT_BUILT_AS_DLL +#include +#endif + +// My classes: +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace mrpt; +using namespace mrpt::viz; +using namespace mrpt::serialization; +using namespace std; + +// Create a set of classes, then serialize and deserialize to test possible +// bugs: +TEST(SerializeTestOpenGL, WriteReadToMem) +{ +#ifndef MRPT_BUILT_AS_DLL + mrpt::viz::registerAllClasses_mrpt_viz(); +#endif + + const mrpt::rtti::TRuntimeClassId* lstClasses[] = { + CLASS_ID(CAxis), + CLASS_ID(CBox), + CLASS_ID(CFrustum), + CLASS_ID(CDisk), + CLASS_ID(CGridPlaneXY), +#if MRPT_HAS_OPENCV // These classes need CImage serialization + CLASS_ID(CMesh), + CLASS_ID(CTexturedPlane), + CLASS_ID(CSkyBox), +#endif + CLASS_ID(Viewport), + CLASS_ID(CPointCloud), + CLASS_ID(CPointCloudColoured), + CLASS_ID(CSetOfLines), + CLASS_ID(CSetOfTriangles), + CLASS_ID(CSphere), + CLASS_ID(CCylinder), + CLASS_ID(CPolyhedron), + CLASS_ID(CArrow), + CLASS_ID(CCamera), + CLASS_ID(CEllipsoid3D), + CLASS_ID(CGridPlaneXZ), + CLASS_ID(Scene), + CLASS_ID(CSetOfObjects), + CLASS_ID(CSimpleLine), + CLASS_ID(CText), + CLASS_ID(CText3D), + CLASS_ID(CEllipsoidInverseDepth2D), + CLASS_ID(CEllipsoidInverseDepth3D), + CLASS_ID(CEllipsoidRangeBearing2D), + CLASS_ID(COctoMapVoxels) + }; + + for (auto& cl : lstClasses) + { + try + { + mrpt::io::CMemoryStream buf; + { + auto o = mrpt::ptr_cast::from(cl->createObject()); + mrpt::serialization::archiveFrom(buf) << *o; + o.reset(); + } + + CSerializable::Ptr recons; + buf.Seek(0); + mrpt::serialization::archiveFrom(buf) >> recons; + } + catch (const std::exception& e) + { + GTEST_FAIL() << "Exception during serialization test for class '" << cl->className << "':\n" + << e.what() << endl; + } + } +} + +TEST(SerializeTestOpenGL, PredefinedSceneFile) +{ + using namespace std::string_literals; + + //! JS_PRELOAD_FILE + const std::string fil = mrpt::UNITTEST_BASEDIR() + "/tests/default-scene.3Dscene"s; + + mrpt::viz::Scene scene; + + ASSERT_FILE_EXISTS_(fil); + bool readOk = scene.loadFromFile(fil); + EXPECT_TRUE(readOk); + + // scene.asYAML().printAsYAML(); + + EXPECT_EQ(scene.viewportsCount(), 2U); + size_t count = 0; + for (const auto& obj : *scene.getViewport()) + { + (void)obj; + count++; + } + + EXPECT_EQ(count, 2U); +} diff --git a/libs/viz/src/viz-precomp.cpp b/libs/viz/src/viz-precomp.cpp new file mode 100644 index 0000000000..a8741c49dd --- /dev/null +++ b/libs/viz/src/viz-precomp.cpp @@ -0,0 +1,10 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "viz-precomp.h" // Precompiled header diff --git a/libs/viz/src/viz-precomp.h b/libs/viz/src/viz-precomp.h new file mode 100644 index 0000000000..01ea0defc1 --- /dev/null +++ b/libs/viz/src/viz-precomp.h @@ -0,0 +1,16 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once +#include +#if MRPT_ENABLE_PRECOMPILED_HDRS + +#include + +#endif