WIP: Adds Tetra-Triangular mesh map

Adds Tetra-Triangular mesh map.
TODO: Code check and add tests

Base/Geometry/imstkCube.cpp

@@ -22,6 +22,7 @@
 #include "imstkCube.h"
 
 namespace imstk {
+
 const double&
 Cube::getWidth() const
 {
@@ -33,4 +34,11 @@ Cube::setWidth(const double& width)
 {
     m_width = width;
 }
+
+double
+Cube::getVolume() const
+{
+    return m_width*m_width;
+}
+
 }

Base/Geometry/imstkCube.h

@@ -42,6 +42,7 @@ public:
     const double& getWidth() const;
     void          setWidth(const double& width);
 
+    double getVolume() const;
 protected:
 
     double m_width;

Base/Geometry/imstkGeometry.cpp

@@ -22,6 +22,7 @@
 #include "imstkGeometry.h"
 
 namespace imstk {
+
 void
 Geometry::translate(const Vec3d& t)
 {

Base/Geometry/imstkGeometry.h

@@ -22,6 +22,7 @@
 #ifndef imstkGeometry_h
 #define imstkGeometry_h
 
+#include "g3log/g3log.hpp"
 #include "imstkMath.h"
 
 namespace imstk {
@@ -71,6 +72,8 @@ public:
 
     const GeometryType& getType() const;
 
+    virtual double getVolume() const = 0;
+
 protected:
 
     Geometry(GeometryType type,

Base/Geometry/imstkGeometryMap.cpp

@@ -22,43 +22,52 @@
 #include "imstkGeometryMap.h"
 
 namespace imstk {
-    void GeometryMap::muteMap()
-    {
-        m_isActive = false;
-    }
-
-    void GeometryMap::activateMap()
-    {
-        m_isActive = true;
-    }
-
-    const GeometryMapType& GeometryMap::getType() const
-    {
-        return m_type;
-    }
-
-    void GeometryMap::setMaster(std::shared_ptr<Geometry> master)
-    {
-        m_master = master;
-    }
-
-    std::shared_ptr<Geometry> GeometryMap::getMaster() const
-    {
-        return m_master;
-    }
-
-    void GeometryMap::setSlave(std::shared_ptr<Geometry> slave)
-    {
-        m_slave = slave;
-    }
-
-    std::shared_ptr<Geometry> GeometryMap::getSlave() const
-    {
-        return m_slave;
-    }
-
-    bool GeometryMap::isActive() const
-    {
-        return m_isActive;
-    }
+
+void
+GeometryMap::muteMap()
+{
+    m_isActive = false;
+}
+
+void
+GeometryMap::activateMap()
+{
+    m_isActive = true;
+}
+
+const
+GeometryMapType& GeometryMap::getType() const
+{
+    return m_type;
+}
+
+void
+GeometryMap::setMaster(std::shared_ptr<Geometry> master)
+{
+    m_master = master;
+}
+
+std::shared_ptr<Geometry>
+GeometryMap::getMaster() const
+{
+    return m_master;
+}
+
+void
+GeometryMap::setSlave(std::shared_ptr<Geometry> slave)
+{
+    m_slave = slave;
+}
+
+std::shared_ptr<Geometry>
+GeometryMap::getSlave() const
+{
+    return m_slave;
+}
+
+bool
+GeometryMap::isActive() const
+{
+    return m_isActive;
+}
 }
\ No newline at end of file

Base/Geometry/imstkGeometryMap.h

@@ -68,10 +68,10 @@ public:
     const GeometryMapType& getType() const;
 
     virtual void setMaster(std::shared_ptr<Geometry> master);
-    std::shared_ptr<Geometry> getMaster() const;
+    virtual std::shared_ptr<Geometry> getMaster() const;
 
     virtual void setSlave(std::shared_ptr<Geometry> slave);
-    std::shared_ptr<Geometry> getSlave() const;
+    virtual std::shared_ptr<Geometry> getSlave() const;
 
     ///
     /// \brief Returns true if the map is actively applied at runtime, else false.

Base/Geometry/imstkHexahedralMesh.cpp

@@ -22,15 +22,77 @@
 #include "imstkHexahedralMesh.h"
 
 namespace imstk {
+
 const std::vector<HexahedralMesh::HexaArray>&
-HexahedralMesh::getHexahedronVertices() const
+HexahedralMesh::getHexahedronMeshVertices() const
 {
     return m_hexahedronVertices;
 }
 
 void
-HexahedralMesh::setHexahedronVertices(const std::vector<HexaArray>& hexahedrons)
+HexahedralMesh::setHexahedronMeshVertices(const std::vector<HexaArray>& hexahedrons)
 {
     m_hexahedronVertices = hexahedrons;
 }
+
+
+const imstk::HexahedralMesh::HexaArray&
+HexahedralMesh::getHexahedronVertices(const int hexaNum) const
+{
+    return m_hexahedronVertices.at(hexaNum);
+}
+
+int
+HexahedralMesh::getNumHexahedra() const
+{
+    return m_hexahedronVertices.size();
+}
+
+double
+HexahedralMesh::getVolume() const
+{
+    double volume = 0.0;
+    imstk::Vec3d v[8];
+    imstk::Vec3d a, b, c;
+    imstk::Mat3d A;
+    for (int i = 0; i < getNumHexahedra(); ++i)
+    {
+        auto hexVerts = getHexahedronVertices(i);
+        for (int i = 0; i < 8; ++i)
+        {
+            v[i] = getVertexPosition(hexVerts[i]);
+        }
+
+        a = v[7] - v[0];
+        b = v[1] - v[0];
+        c = v[3] - v[5];
+
+        A << a[0], b[0], c[0],
+            a[1], b[1], c[1],
+            a[2], b[2], c[2];
+
+        volume += A.determinant();
+
+        b = v[4] - v[0];
+        c = v[5] - v[6];
+
+        A << a[0], b[0], c[0],
+            a[1], b[1], c[1],
+            a[2], b[2], c[2];
+
+        volume += A.determinant();
+
+        b = v[2] - v[0];
+        c = v[6] - v[3];
+
+        A << a[0], b[0], c[0],
+            a[1], b[1], c[1],
+            a[2], b[2], c[2];
+
+        volume += A.determinant();
+    }
+
+    return volume/6;
+}
+
 }

Base/Geometry/imstkHexahedralMesh.h

@@ -37,8 +37,16 @@ public:
 
     ~HexahedralMesh() = default;
 
-    const std::vector<HexaArray>& getHexahedronVertices() const;
-    void                          setHexahedronVertices(const std::vector<HexaArray>& hexahedrons);
+    const std::vector<HexaArray>& getHexahedronMeshVertices() const;
+    void                          setHexahedronMeshVertices(const std::vector<HexaArray>& hexahedrons);
+
+    const imstk::HexahedralMesh::HexaArray& getHexahedronVertices(const int hexaNum) const;
+    int getNumHexahedra() const;
+
+    ///
+    /// \brief Get the total volume of the hexahedral mesh
+    ///
+    double getVolume() const;
 
 protected:
 

Base/Geometry/imstkIdentityMap.cpp

@@ -23,24 +23,27 @@
 
 namespace imstk {
 
-    const imstk::RigidTransform3d& IdentityMap::getTransform() const
+const imstk::RigidTransform3d&
+IdentityMap::getTransform() const
+{
+    return imstk::RigidTransform3d::Identity();
+}
+
+void
+IdentityMap::applyMap()
+{
+    if (m_isActive)
     {
-        return imstk::RigidTransform3d::Identity();
-    }
-
-    void IdentityMap::applyMap()
-    {
-        if (m_isActive)
+        if (!m_master || !m_slave)
         {
-            if (!m_master || !m_slave)
-            {
-                LOG(WARNING) << "Identity map is being applied without valid geometries\n";
-                return;
-            }
-
-            // Set the follower mesh configuration to be same as that of master
-            m_slave->setPosition(m_master->getPosition());
-            m_slave->setOrientation(m_master->getOrientation());
+            LOG(WARNING) << "Identity map is being applied without valid geometries\n";
+            return;
         }
+
+        // Set the follower mesh configuration to be same as that of master
+        m_slave->setPosition(m_master->getPosition());
+        m_slave->setOrientation(m_master->getOrientation());
     }
+}
+
 }
\ No newline at end of file

Base/Geometry/imstkIsometricMap.cpp

@@ -21,32 +21,37 @@
 
 #include "imstkIsometricMap.h"
 namespace imstk {
-    void IsometricMap::setTransform(const RigidTransform3d& affineTransform)
-    {
-        m_rigidTransform = affineTransform;
-    }
-
-    const imstk::RigidTransform3d& IsometricMap::getTransform() const
-    {
-        return m_rigidTransform;
-    }
 
-    void IsometricMap::applyMap()
+void
+IsometricMap::setTransform(const RigidTransform3d& affineTransform)
+{
+    m_rigidTransform = affineTransform;
+}
+
+const imstk::RigidTransform3d&
+IsometricMap::getTransform() const
+{
+    return m_rigidTransform;
+}
+
+void
+IsometricMap::applyMap()
+{
+    if (m_isActive)
     {
-        if (m_isActive)
+        if (!m_master || !m_slave)
         {
-            if (!m_master || !m_slave)
-            {
-                //LOG(WARNING) << "Isometric map is being applied without valid geometries\n";
-                return;
-            }
-
-            // First set the follower mesh configuration to that of master
-            m_slave->setPosition(m_master->getPosition());
-            m_slave->setOrientation(m_master->getOrientation());
-
-            // Now, apply the offset transform
-            m_slave->transform(m_rigidTransform);
+            LOG(WARNING) << "Isometric map is being applied without valid geometries\n";
+            return;
         }
+
+        // First set the follower mesh configuration to that of master
+        m_slave->setPosition(m_master->getPosition());
+        m_slave->setOrientation(m_master->getOrientation());
+
+        // Now, apply the offset transform
+        m_slave->transform(m_rigidTransform);
     }
+}
+
 }
\ No newline at end of file

Base/Geometry/imstkMesh.cpp

@@ -28,6 +28,11 @@ Mesh::getInitialVertexPositions() const
     return m_initialVertexPositions;
 }
 
+const imstk::Vec3d& Mesh::getInitialVertexPosition(const int vertNum) const
+{
+    return m_initialVertexPositions.at(vertNum);
+}
+
 void
 Mesh::setInitialVertexPositions(const std::vector<Vec3d>& vertices)
 {
@@ -46,6 +51,16 @@ Mesh::setVertexPositions(const std::vector<Vec3d>& vertices)
     m_vertexPositions = vertices;
 }
 
+const imstk::Vec3d& Mesh::getVertexPosition(const int vertNum) const
+{
+    return m_vertexPositions.at(vertNum);
+}
+
+void Mesh::setVertexPosition(const int vertNum, const imstk::Vec3d& pos)
+{
+    m_vertexPositions.at(vertNum) = pos;
+}
+
 const std::vector<Vec3d>&
 Mesh::getVertexDisplacements() const
 {
@@ -57,4 +72,46 @@ Mesh::setVertexDisplacements(const std::vector<Vec3d>& diff)
 {
     m_vertexDisplacements = diff;
 }
+
+const imstk::Vec3d& Mesh::getVertexDisplacement(const int vertNum) const
+{
+    return m_vertexDisplacements.at(vertNum);
+}
+
+int Mesh::getNumVertices() const
+{
+    return m_initialVertexPositions.size();
+}
+
+void Mesh::computeBoundingBox(imstk::Vec3d& min, imstk::Vec3d& max, const double percent) const
+{
+    min = imstk::Vec3d(std::numeric_limits<double>::max(),
+        std::numeric_limits<double>::max(),
+        std::numeric_limits<double>::max());
+
+    max = imstk::Vec3d(std::numeric_limits<double>::min(),
+        std::numeric_limits<double>::min(),
+        std::numeric_limits<double>::min());
+
+    for (auto it = m_vertexPositions.begin(); it != m_vertexPositions.end(); ++it)
+    {
+        for (int i = 0; i < 3; ++i)
+        {
+            min[i] = std::min(min[i], (*it)[i]);
+            max[i] = std::max(max[i], (*it)[i]);
+        }
+    }
+
+    if (percent == 0.0)
+    {
+        return;
+    }
+    else
+    {
+        imstk::Vec3d range = max - min;
+        min = min - range*(percent / 100);
+        max = max + range*(percent / 100);
+    }
+}
+
 }

Base/Geometry/imstkMesh.h

@@ -31,15 +31,27 @@ public:
 
     ~Mesh() = default;
 
+    // Accessors
     const std::vector<Vec3d>& getInitialVertexPositions() const;
     void                      setInitialVertexPositions(const std::vector<Vec3d>& vertices);
 
+    const imstk::Vec3d& getInitialVertexPosition(const int vertNum) const;
+
     const std::vector<Vec3d>& getVertexPositions() const;
     void                      setVertexPositions(const std::vector<Vec3d>& vertices);
 
+    const imstk::Vec3d& getVertexPosition(const int vertNum) const;
+    void setVertexPosition(const int vertNum, const imstk::Vec3d& pos);
+
     const std::vector<Vec3d>& getVertexDisplacements() const;
     void                      setVertexDisplacements(const std::vector<Vec3d>& diff);
 
+    const imstk::Vec3d& getVertexDisplacement(const int vertNum) const;
+
+    int getNumVertices() const;
+
+    void computeBoundingBox(imstk::Vec3d& min, imstk::Vec3d& max, const double percent = 0.0) const;
+
 protected:
 
     Mesh(GeometryType type) : Geometry(type, WORLD_ORIGIN, Quatd()) {}

Base/Geometry/imstkPlane.cpp

@@ -45,4 +45,10 @@ Plane::setWidth(const double& width)
 {
     m_width = width;
 }
+
+double
+Plane::getVolume() const
+{
+    return 0.0;
+}
 }

Base/Geometry/imstkPlane.h

@@ -46,6 +46,7 @@ public:
     const double& getWidth() const;
     void          setWidth(const double& width);
 
+    double getVolume() const;
 protected:
 
     double m_width;

Base/Geometry/imstkSphere.cpp

@@ -33,4 +33,11 @@ Sphere::setRadius(const double& radius)
 {
     m_radius = radius;
 }
+
+double
+Sphere::getVolume() const
+{
+    return 0.75*imstk::PI*m_radius*m_radius*m_radius;
+}
+
 }

Base/Geometry/imstkSphere.h

@@ -42,6 +42,7 @@ public:
     const double& getRadius() const;
     void          setRadius(const double& radius);
 
+    double getVolume() const;
 protected:
 
     double m_radius;

Base/Geometry/imstkSurfaceMesh.cpp

@@ -237,4 +237,27 @@ SurfaceMesh::getVertexTangent(size_t i) const
 {
     return m_vertexTangents.at(i);
 }
+
+const imstk::Vec3d & SurfaceMesh::getVertexInitialPosition(size_t i) const
+{
+    return m_initialVertexPositions.at(i);
+}
+
+const imstk::Vec3d & SurfaceMesh::getVertexPosition(size_t i) const
+{
+    return m_vertexPositions.at(i);
+}
+
+double
+SurfaceMesh::getVolume() const
+{
+    // TODO
+    // 1. Check for water tightness
+    // 2. Compute volume based on signed distance
+
+    LOG(WARNING) << "Not supported yet, returns 0.0!\n";
+
+    return 0.0;
+}
+
 }

Base/Geometry/imstkSurfaceMesh.h

@@ -62,6 +62,10 @@ public:
     const std::vector<Vec4d>& getVertexTangents() const;
     const Vec4d             & getVertexTangent(size_t i) const;
 
+    const Vec3d             & getVertexInitialPosition(size_t i) const;
+    const Vec3d             & getVertexPosition(size_t i) const;
+
+    double getVolume() const;
 protected:
 
     std::vector<TriangleArray> m_triangleVertices;

Base/Geometry/imstkTetraTriangleMap.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+      http://www.apache.org/licenses/LICENSE-2.0.txt
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+
+   =========================================================================*/
+
+#include "imstkTetraTriangleMap.h"
+
+namespace imstk
+{
+
+void
+TetraTriangleMap::computeMap()
+{
+    if (!m_master || !m_slave)
+    {
+        LOG(WARNING) << "TetraTriangle map is being applied without valid geometries\n";
+        return;
+    }
+
+    // Proceed to generate the map
+    auto tetMesh = std::dynamic_pointer_cast<imstk::TetrahedralMesh> (m_master);
+    auto triMesh = std::dynamic_pointer_cast<imstk::SurfaceMesh> (m_slave);
+
+    weightsArray weights;
+    int numSurfaceVertices = triMesh->getNumVertices();
+    int numTetrahedra = tetMesh->getNumTetrahedra();
+
+    for (int i = 0; i < numSurfaceVertices; ++i)
+    {
+        imstk::Vec3d surfVertPos = triMesh->getVertexInitialPosition(i);
+
+        // find the enclosing element
+        int closestEle = findEclosingTetrahedra(tetMesh, surfVertPos);
+
+        // if not inside of any element,
+        // find the tetrahedra whose centroid is the closest
+        if (closestEle < 0)
+        {
+            closestEle = findClosestTetrahedra(tetMesh, surfVertPos);
+        }
+