Added mesh modifier in bioptim

CMakeLists.txt

@@ -3,7 +3,7 @@ if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.0")
     cmake_policy(SET CMP0042 NEW)
 endif()
 
-project(biorbd VERSION 1.7.3)
+project(biorbd VERSION 1.7.4)
 set(BIORBD_ROOT_FOLDER ${PROJECT_SOURCE_DIR})
 set (BIORBD_NAME_NO_SUFFIX ${PROJECT_NAME})
 set (CMAKE_CXX_STANDARD 11)

README.md

@@ -418,11 +418,21 @@ When calculating the inverse dynamics, if force plates are used, this tag dispat
 Warning: this tag MUST be added to a segment that has a translation and/or a rotation (i.e. that possesses at least one degree of freedom). Otherwise, it will simply be ignored
 
 ##### meshfile or ply
-The path of the meshing `.bioBone` or `.ply` file respectively. It can be relative to the current running folder or absolute (relative being preferred) and UNIX or Windows formatted (`/` vs `\\`, UNIX being preferred).
+The path of the meshing `.bioBone`, `.ply`, `.stl` file respectively. It can be relative to the current running folder or absolute (relative being preferred) and UNIX or Windows formatted (`/` vs `\\`, UNIX being preferred).
 
 ##### mesh
 If the mesh is not written in a file, it can be written directly in the segment. If so, the `mesh` tag stands for the vertex. Therefore, there are as many `mesh` tags as vertex. It waits for $3$ values being the position relative to reference of the segment. 
 
+##### meshcolor
+The color of the segment mesh given in RGV values `[0, 1]`. Default is `0.89, 0.855, 0.788`, that is bone color-ish.
+
+##### meshscale
+The scaling to apply to the provided mesh, given in `X Y Z` values. Default is `1 1 1`.
+
+##### meshrt
+The RT to apply to the provided mesh, given in `RX RY RZ seq TX TY TZ` as for RT. The default value is `0 0 0 xyz 0 0 0`. 
+
+
 ##### patch
 The patches to define the orientation of the patches of the mesh. It waits for $3$ values being the $0-based$ of the index of the vertex defined by the `mesh`.
 

include/RigidBody/Joints.h

@@ -709,7 +709,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model
         bool updateKin);
 
     ///
-    /// \brief Calculate the segment center of mass angular momentum
+    /// \brief Calculate the segment center of mass angular momentum with respect to the global center of mass
     /// \param Q The generalized coordinates
     /// \param Qdot The generalized velocities
     /// \param updateKin If the kinematics of the model should be computed
@@ -721,7 +721,7 @@ class BIORBD_API Joints : public RigidBodyDynamics::Model
         bool updateKin);
 
     ///
-    /// \brief Calculate the segment center of mass angular momentum
+    /// \brief Calculate the segment center of mass angular momentum with respect to the global center of mass
     /// \param Q The generalized coordinates
     /// \param Qdot The generalized velocities
     /// \param Qddot The generalized accelerations

include/RigidBody/Mesh.h

@@ -10,6 +10,7 @@ namespace BIORBD_NAMESPACE
 namespace utils
 {
 class Vector3d;
+class RotoTrans;
 class Path;
 }
 
@@ -57,6 +58,19 @@ class BIORBD_API Mesh
     void DeepCopy(
         const Mesh& other);
 
+    ///
+    /// \brief Set the patch color
+    /// \param color The color
+    ///
+    void setColor(
+            const utils::Vector3d& color);
+
+    ///
+    /// \brief Get the patch color
+    /// \return The patch color
+    ///
+    utils::Vector3d& color() const;
+
     ///
     /// \brief Add a point to the mesh
     /// \param node The point to add
@@ -78,6 +92,22 @@ class BIORBD_API Mesh
     ///
     unsigned int nbVertex() const;
 
+    ///
+    /// \brief rotateVertex Apply the RT to the vertex
+    /// \param rt The Transformation to apply to the mesh wrt the parent
+    ///
+    void rotate(
+            const utils::RotoTrans& rt);
+
+    ///
+    /// \brief Scale the vertex wrt to 0, 0, 0. Warnign, this function is
+    /// applied when called, meaning that if it is called after rotate, it will
+    /// results in surprising results!
+    /// \param scaler The x, y, z values to scale
+    ///
+    void scale(
+            const utils::Vector3d& scaler);
+
     ///
     /// \brief Add a face patch to the mesh
     /// \param face The face patch to add
@@ -129,6 +159,7 @@ class BIORBD_API Mesh
     std::shared_ptr<std::vector<MeshFace>>
             m_faces; ///< The faces
     std::shared_ptr<utils::Path> m_pathFile; ///< The path to the mesh file
+    std::shared_ptr<utils::Vector3d> m_patchColor; ///< The color of faces
 };
 
 }

src/ModelReader.cpp

@@ -115,6 +115,7 @@ void Reader::readModelFile(
                 utils::RotoTrans RT(RigidBodyDynamics::Math::Matrix4d::Identity());
                 utils::Vector3d com(0,0,0);
                 rigidbody::Mesh mesh;
+                bool isMeshSet(false);
                 int segmentByFile(-1); // -1 non setté, 0 pas par file, 1 par file
                 int PF = -1;
                 std::vector<utils::Range> QRanges;
@@ -224,6 +225,7 @@ void Reader::readModelFile(
                         utils::Vector3d tp(0, 0, 0);
                         readVector3d(file, variable, tp);
                         mesh.addPoint(tp);
+                        isMeshSet = true;
                     } else if (!property_tag.tolower().compare("patch")) {
                         if (segmentByFile==-1) {
                             segmentByFile = 0;
@@ -263,6 +265,21 @@ void Reader::readModelFile(
                             utils::Error::raise(filePath.extension() +
                                                         " is an unrecognized mesh file");
                         }
+                        isMeshSet = true;
+                    } else if (!property_tag.tolower().compare("meshrt")) {
+                        utils::Error::check(isMeshSet, "mesh(es) or meshfile should be declared before meshrt");
+                        utils::RotoTrans meshRT(RigidBodyDynamics::Math::Matrix4d::Identity());
+                        readRtMatrix(file, variable, false, meshRT);
+                        mesh.rotate(meshRT);
+                    } else if (!property_tag.tolower().compare("meshscale")) {
+                        utils::Error::check(isMeshSet, "mesh(es) or meshfile should be declared before meshscale");
+                        utils::Vector3d scaler;
+                        readVector3d(file, variable, scaler);
+                        mesh.scale(scaler);
+                    } else if (!property_tag.tolower().compare("meshcolor")){
+                        utils::Vector3d meshColor;
+                        readVector3d(file, variable, meshColor);
+                        mesh.setColor(meshColor);
                     }
                 }
                 if (!isRangeQSet) {
@@ -1931,6 +1948,7 @@ rigidbody::Mesh Reader::readMeshFileVtp(
 
     return mesh;
 }
+#endif  // MODULE_VTP_FILES_READER
 
 void Reader::readVector3d(
     utils::IfStream &file,
@@ -1985,7 +2003,6 @@ void Reader::readRtMatrix(
     }
     RT.checkUnitary();
 }
-#endif  // MODULE_VTP_FILES_READER
 
 rigidbody::Mesh Reader::readMeshFileStl(
     const utils::Path &path)

src/RigidBody/Mesh.cpp

@@ -3,23 +3,26 @@
 
 #include "Utils/Path.h"
 #include "Utils/Vector3d.h"
+#include "Utils/RotoTrans.h"
 #include "RigidBody/MeshFace.h"
 
 using namespace BIORBD_NAMESPACE;
 
 rigidbody::Mesh::Mesh() :
     m_vertex(std::make_shared<std::vector<utils::Vector3d>>()),
     m_faces(std::make_shared<std::vector<rigidbody::MeshFace>>()),
-    m_pathFile(std::make_shared<utils::Path>())
+    m_pathFile(std::make_shared<utils::Path>()),
+    m_patchColor(std::make_shared<utils::Vector3d>(0.89, 0.855, 0.788))
 {
 
 }
 
-rigidbody::Mesh::Mesh(const std::vector<utils::Vector3d> &other)
-    :
+rigidbody::Mesh::Mesh(
+        const std::vector<utils::Vector3d> &other):
     m_vertex(std::make_shared<std::vector<utils::Vector3d>>(other)),
     m_faces(std::make_shared<std::vector<rigidbody::MeshFace>>()),
-    m_pathFile(std::make_shared<utils::Path>())
+    m_pathFile(std::make_shared<utils::Path>()),
+    m_patchColor(std::make_shared<utils::Vector3d>(0.89, 0.855, 0.788))
 {
 
 }
@@ -29,7 +32,8 @@ rigidbody::Mesh::Mesh(const std::vector<utils::Vector3d>
                               const std::vector<rigidbody::MeshFace> & faces) :
     m_vertex(std::make_shared<std::vector<utils::Vector3d>>(vertex)),
     m_faces(std::make_shared<std::vector<rigidbody::MeshFace>>(faces)),
-    m_pathFile(std::make_shared<utils::Path>())
+    m_pathFile(std::make_shared<utils::Path>()),
+    m_patchColor(std::make_shared<utils::Vector3d>(0.89, 0.855, 0.788))
 {
 
 }
@@ -54,6 +58,17 @@ void rigidbody::Mesh::DeepCopy(const rigidbody::Mesh &other)
     *m_pathFile = other.m_pathFile->DeepCopy();
 }
 
+void rigidbody::Mesh::setColor(
+        const utils::Vector3d &color)
+{
+    *m_patchColor = color;
+}
+
+utils::Vector3d &rigidbody::Mesh::color() const
+{
+    return *m_patchColor;
+}
+
 void rigidbody::Mesh::addPoint(const utils::Vector3d &node)
 {
     m_vertex->push_back(node);
@@ -68,6 +83,24 @@ unsigned int rigidbody::Mesh::nbVertex() const
     return static_cast<unsigned int>(m_vertex->size());
 }
 
+void rigidbody::Mesh::rotate(
+        const utils::RotoTrans &rt)
+{
+    for (auto& v : *m_vertex){
+        v.applyRT(rt);
+    }
+}
+
+void rigidbody::Mesh::scale(
+        const utils::Vector3d &scaler)
+{
+    for (auto& v: *m_vertex){
+        v(0) *= scaler(0);
+        v(1) *= scaler(1);
+        v(2) *= scaler(2);
+    }
+}
+
 unsigned int rigidbody::Mesh::nbFaces()
 {
     return static_cast<unsigned int>(m_faces->size());

test/test_rigidbody.cpp

@@ -832,6 +832,105 @@ static std::vector<std::vector<double>> expectedMarkers = {
     std::vector<double>({0.39552020666133958, -0.18232123669751762,   1.0126678074548392}),
     std::vector<double>({1.0258667774186612, 1.0702910100794407, 1.1960410878390473})
 };
+
+TEST(Mesh, scale)
+{
+    rigidbody::Mesh mesh;
+    mesh.addPoint(utils::Vector3d(2, 3, 4));
+    mesh.addPoint(utils::Vector3d(5, 6, 7));
+
+    mesh.scale(utils::Vector3d(2, 3, 4));
+
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(0)[0]);
+        EXPECT_FLOAT_EQ(val, 4);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(0)[1]);
+        EXPECT_FLOAT_EQ(val, 9);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(0)[2]);
+        EXPECT_FLOAT_EQ(val, 16);
+    }
+
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(1)[0]);
+        EXPECT_FLOAT_EQ(val, 10);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(1)[1]);
+        EXPECT_FLOAT_EQ(val, 18);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(1)[2]);
+        EXPECT_FLOAT_EQ(val, 28);
+    }
+
+    mesh.rotate(utils::RotoTrans(
+                    utils::Vector3d(0.2, 0.3, 0.4),
+                    utils::Vector3d(2, 3, 4),
+                    "xyz"));
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(0)[0]);
+        EXPECT_FLOAT_EQ(val, 6.899786);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(0)[1]);
+        EXPECT_FLOAT_EQ(val, 9.6247339);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(0)[2]);
+        EXPECT_FLOAT_EQ(val, 20.885052);
+    }
+
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(1)[0]);
+        EXPECT_FLOAT_EQ(val, 12.377337);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(1)[1]);
+        EXPECT_FLOAT_EQ(val, 17.880116);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, mesh.point(1)[2]);
+        EXPECT_FLOAT_EQ(val, 33.64613);
+    }
+}
+
+TEST(Mesh, color){
+    rigidbody::Mesh mesh;
+    utils::Vector3d color(mesh.color());
+    {
+        SCALAR_TO_DOUBLE(val, color[0]);
+        EXPECT_FLOAT_EQ(val, 0.89);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, color[1]);
+        EXPECT_FLOAT_EQ(val, 0.855);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, color[2]);
+        EXPECT_FLOAT_EQ(val, 0.788);
+    }
+
+    mesh.setColor(utils::Vector3d(0.1, 0.2, 0.3));
+    color = mesh.color();
+    {
+        SCALAR_TO_DOUBLE(val, color[0]);
+        EXPECT_FLOAT_EQ(val, 0.1);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, color[1]);
+        EXPECT_FLOAT_EQ(val, 0.2);
+    }
+    {
+        SCALAR_TO_DOUBLE(val, color[2]);
+        EXPECT_FLOAT_EQ(val, 0.3);
+    }
+
+}
+
 TEST(Markers, allPositions)
 {
     Model model(modelPathMeshEqualsMarker);