WIP: Adds one to one nodal map

Adds one to one nodal map between two meshes. There are compute and set options for the map.

Base/Geometry/Map/imstkGeometryMap.h

@@ -34,6 +34,7 @@ enum class GeometryMapType
 {
     Isometric,
     Identity,
+    OneToOne,
     TetraTriangle,
     HexaTriangle,
     TetraTetra
@@ -65,6 +66,11 @@ public:
     ///
     void mute();
 
+    ///
+    /// \brief Check the validity of the map
+    ///
+    virtual bool isValid() const = 0;
+
     ///
     /// \brief Activate the map
     ///

Base/Geometry/Map/imstkIdentityMap.h

@@ -44,6 +44,11 @@ public:
     ///
     void apply() override;
 
+    ///
+    /// \brief Check the validity of the map
+    ///
+    bool isValid() const { return true; };
+
     // Accessors
     void setTransform(const RigidTransform3d& affineTransform) = delete;
     const RigidTransform3d getTransform() const;

Base/Geometry/Map/imstkIsometricMap.h

@@ -47,6 +47,11 @@ public:
     ///
     void apply() override;
 
+    ///
+    /// \brief Check the validity of the map
+    ///
+    bool isValid() const { return true; };
+
     // Accessors
     void setTransform(const RigidTransform3d& affineTransform);
     const RigidTransform3d& getTransform() const;

Base/Geometry/Map/imstkOneToOneMap.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 "imstkOneToOneMap.h"
+
+namespace imstk
+{
+
+void
+OneToOneMap::compute()
+{
+    if (!m_master || !m_slave)
+    {
+        LOG(WARNING) << "OneToOneMap map is being applied without valid geometries";
+        return;
+    }
+
+    auto meshMaster = std::dynamic_pointer_cast<Mesh>(m_master);
+    auto meshSlave = std::dynamic_pointer_cast<Mesh>(m_slave);
+
+    m_oneToOneMap.clear();
+    for (size_t nodeId = 0; nodeId < meshSlave->getNumVertices(); ++nodeId)
+    {
+        // Find the enclosing or closest tetrahedron
+        int matchingNodeId = findMatchingVertex(meshMaster, meshSlave->getInitialVertexPosition(nodeId));
+
+        if (matchingNodeId < 0)
+        {
+            LOG(WARNING) << "Could not find matching node for the node " << nodeId;
+            continue;
+        }
+
+        // add to the map
+        // Note: This replaces the map if one with <nodeId> already exists
+        m_oneToOneMap[nodeId] = matchingNodeId;
+    }
+}
+
+int
+OneToOneMap::findMatchingVertex(std::shared_ptr<Mesh> masterMesh, const Vec3d& p) const
+{
+    // returns the first matching vertex
+    for (size_t nodeId = 0; nodeId < masterMesh->getNumVertices(); ++nodeId)
+    {
+        if (masterMesh->getInitialVertexPosition(nodeId) == p)
+        {
+            return nodeId;
+        }
+    }
+    return -1;
+}
+
+bool
+OneToOneMap::isValid() const
+{
+    auto meshMaster = std::dynamic_pointer_cast<Mesh> (m_master);
+    auto meshSlave = std::dynamic_pointer_cast<Mesh> (m_slave);
+
+    auto numVertMaster = meshMaster->getNumVertices();
+    auto numVertSlave = meshSlave->getNumVertices();
+
+    for (auto const& mapValue : m_oneToOneMap)
+    {
+        if (mapValue.first >= 0 && mapValue.first < numVertSlave &&
+            mapValue.second >= 0 && mapValue.second < numVertMaster)
+        {
+            continue;
+        }
+        else
+        {
+            LOG(WARNING) << "OneToOneMap map is not valid! Vertex indices out of bounds.";
+            return false;
+        }
+    }
+    return true;
+}
+
+void
+OneToOneMap::setMap(const std::map<int, int>& sourceMap)
+{
+    this->m_oneToOneMap = sourceMap;
+}
+
+void
+OneToOneMap::apply()
+{
+    // Check Map active
+    if (!m_isActive)
+    {
+        LOG(WARNING) << "OneToOneMap map is not active";
+        return;
+    }
+
+    // Check geometries
+    if (!m_master || !m_slave)
+    {
+        LOG(WARNING) << "OneToOneMap map is being applied without valid geometries";
+        return;
+    }
+
+    auto meshMaster = std::dynamic_pointer_cast<Mesh>(m_master);
+    auto meshSlave = std::dynamic_pointer_cast<Mesh>(m_slave);
+
+    for (auto const& mapValue : m_oneToOneMap)
+    {
+        meshSlave->setVerticePosition(mapValue.first, meshMaster->getVertexPosition(mapValue.second));
+    }
+}
+
+void
+OneToOneMap::print() const
+{
+    // Print Type
+    GeometryMap::print();
+
+    // Print the one-to-one map
+    LOG(INFO) << "[vertexId1, vertexId2]\n";
+    for (auto const& mapValue : this->m_oneToOneMap)
+    {
+        LOG(INFO) << "[" << mapValue.first << ", " << mapValue.second << "]\n";
+    }
+}
+
+void
+OneToOneMap::setMaster(std::shared_ptr<Geometry> master)
+{
+    if (master->getType() == GeometryType::HexahedralMesh ||
+        master->getType() == GeometryType::SurfaceMesh ||
+        master->getType() == GeometryType::TetrahedralMesh)
+    {
+        LOG(WARNING) << "The geometry provided is not a mesh!\n";
+        return;
+    }
+    GeometryMap::setMaster(master);
+}
+
+void
+OneToOneMap::setSlave(std::shared_ptr<Geometry> slave)
+{
+    if (slave->getType() == GeometryType::HexahedralMesh ||
+        slave->getType() == GeometryType::SurfaceMesh ||
+        slave->getType() == GeometryType::TetrahedralMesh)
+    {
+        LOG(WARNING) << "The geometry provided is not a mesh!\n";
+        return;
+    }
+    GeometryMap::setSlave(slave);
+}
+
+}

Base/Geometry/Map/imstkOneToOneMap.h

+/*=========================================================================
+
+   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.
+
+   =========================================================================*/
+
+#ifndef imstkOneToOneMap_h
+#define imstkOneToOneMap_h
+
+#include <limits>
+
+#include "imstkGeometryMap.h"
+#include "imstkTetrahedralMesh.h"
+#include "imstkSurfaceMesh.h"
+
+namespace imstk {
+
+///
+/// \class OneToOneMap
+///
+/// \brief Computes and applies the One-to-one map. The master and the slave geometries
+/// should contain nodes.
+///
+class OneToOneMap : public GeometryMap
+{
+public:
+
+    OneToOneMap() : GeometryMap(GeometryMapType::OneToOne){}
+
+    ~OneToOneMap() = default;
+
+    ///
+    /// \brief Compute the tetra-triangle mesh map
+    ///
+    void compute() override;
+
+    ///
+    /// \brief Find the matching node
+    ///
+    int findMatchingVertex(std::shared_ptr<Mesh> tetraMesh, const Vec3d& p) const;
+
+    ///
+    /// \brief Check the validity of the map
+    ///
+    bool isValid() const;
+
+    ///
+    /// \brief Sets the one-to-one correspondence directly
+    ///
+    void setMap(const std::map<int, int>& sourceMap);
+
+    ///
+    /// \brief Apply (if active) the tetra-triangle mesh map
+    ///
+    void apply() override;
+
+    ///
+    /// \brief Print the map
+    ///
+    void print() const override;
+
+    ///
+    /// \brief Set the geometry that dictates the map
+    ///
+    void setMaster(std::shared_ptr<Geometry> master) override;
+
+    ///
+    /// \brief Set the geometry that follows the master
+    ///
+    void setSlave(std::shared_ptr<Geometry> slave) override;
+
+protected:
+    std::map<int, int> m_oneToOneMap;
+};
+}
+
+#endif // imstkOneToOneMap_h

Base/Geometry/Map/imstkTetraTriangleMap.cpp

@@ -36,6 +36,8 @@ TetraTriangleMap::compute()
     auto tetMesh = std::dynamic_pointer_cast<TetrahedralMesh> (m_master);
     auto triMesh = std::dynamic_pointer_cast<SurfaceMesh> (m_slave);
 
+    m_verticesEnclosingTetraId.clear();
+    m_verticesWeights.clear();
     for (const Vec3d& surfVertPos : triMesh->getInitialVerticesPositions())
     {
         // Find the enclosing or closest tetrahedron
@@ -88,7 +90,7 @@ TetraTriangleMap::apply()
         auto weights = m_verticesWeights.at(vertexId);
         for (size_t i = 0; i < 4; ++i)
         {
-            newPos += tetMesh->getInitialVerticePosition(tetVerts[i]) * weights[i];
+            newPos += tetMesh->getInitialVertexPosition(tetVerts[i]) * weights[i];
         }
         triMesh->setVerticePosition(vertexId, newPos);
     }
@@ -114,6 +116,23 @@ TetraTriangleMap::print() const
     }
 }
 
+bool
+TetraTriangleMap::isValid() const
+{
+    auto meshMaster = std::dynamic_pointer_cast<TetrahedralMesh>(m_master);
+    auto totalElementsMaster = meshMaster->getNumTetrahedra();
+
+    for (size_t tetId = 0; tetId < m_verticesEnclosingTetraId.size(); ++tetId)
+    {
+        if (!(m_verticesEnclosingTetraId.at(tetId) < totalElementsMaster &&
+            m_verticesEnclosingTetraId.at(tetId) >= 0))
+        {
+            return false;
+        }
+    }
+    return true;
+}
+
 void
 TetraTriangleMap::setMaster(std::shared_ptr<Geometry> master)
 {
@@ -148,7 +167,7 @@ TetraTriangleMap::findClosestTetrahedron(std::shared_ptr<TetrahedralMesh> tetraM
         auto vert = tetraMesh->getTetrahedronVertices(tetId);
         for (size_t i = 0; i < 4; ++i)
         {
-            center += tetraMesh->getInitialVerticePosition(vert[i]);
+            center += tetraMesh->getInitialVertexPosition(vert[i]);
         }
 
         double dist = (pos - center).norm();

Base/Geometry/Map/imstkTetraTriangleMap.h

@@ -59,6 +59,11 @@ public:
     ///
     void print() const override;
 
+    ///
+    /// \brief Check the validity of the map
+    ///
+    bool isValid() const;
+
     ///
     /// \brief Set the geometry that dictates the map
     ///

Base/Geometry/Mesh/imstkHexahedralMesh.cpp

@@ -57,7 +57,7 @@ HexahedralMesh::getVolume() const
     {
         for (int i = 0; i < 8; ++i)
         {
-            v[i] = this->getVerticePosition(hexArray[i]);
+            v[i] = this->getVertexPosition(hexArray[i]);
         }
 
         a = v[7] - v[0];

Base/Geometry/Mesh/imstkMesh.cpp

@@ -35,7 +35,7 @@ Mesh::getInitialVerticesPositions() const
 }
 
 const Vec3d&
-Mesh::getInitialVerticePosition(const int& vertNum) const
+Mesh::getInitialVertexPosition(const int& vertNum) const
 {
     return m_initialVerticesPositions.at(vertNum);
 }
@@ -58,7 +58,7 @@ void Mesh::setVerticePosition(const int& vertNum, const Vec3d& pos)
 }
 
 const Vec3d&
-Mesh::getVerticePosition(const int& vertNum) const
+Mesh::getVertexPosition(const int& vertNum) const
 {
     return m_verticesPositions.at(vertNum);
 }

Base/Geometry/Mesh/imstkMesh.h

@@ -34,12 +34,12 @@ public:
     // Accessors
     void setInitialVerticesPositions(const std::vector<Vec3d>& vertices);
     const std::vector<Vec3d>& getInitialVerticesPositions() const;
-    const Vec3d& getInitialVerticePosition(const int& vertNum) const;
+    const Vec3d& getInitialVertexPosition(const int& vertNum) const;
 
     void setVerticesPositions(const std::vector<Vec3d>& vertices);
     const std::vector<Vec3d>& getVerticesPositions() const;
     void setVerticePosition(const int &vertNum, const Vec3d& pos);
-    const Vec3d& getVerticePosition(const int& vertNum) const;
+    const Vec3d& getVertexPosition(const int& vertNum) const;
 
     void setVerticesDisplacements(const std::vector<Vec3d>& diff);
     const std::vector<Vec3d>& getVerticesDisplacements() const;

Base/Geometry/Mesh/imstkTetrahedralMesh.cpp

@@ -55,7 +55,7 @@ TetrahedralMesh::getVolume() const
     {
         for (int i = 0; i < 4; i++)
         {
-            v[i] = this->getVerticePosition(tetVertices[i]);
+            v[i] = this->getVertexPosition(tetVertices[i]);
         }
 
         A << v[0][0], v[0][1], v[0][2], 1,
@@ -85,7 +85,7 @@ TetrahedralMesh::computeBarycentricWeights(const size_t& tetId, const Vec3d& pos
 
     for (int i = 0; i < 4; i++)
     {
-        v[i] = this->getVerticePosition(tetVertices[i]);
+        v[i] = this->getVertexPosition(tetVertices[i]);
     }
 
     Mat4d A;
@@ -109,10 +109,10 @@ TetrahedralMesh::computeBarycentricWeights(const size_t& tetId, const Vec3d& pos
 void
 TetrahedralMesh::computeTetrahedronBoundingBox(const size_t& tetId, Vec3d& min, Vec3d& max) const
 {
-    auto v1 = this->getVerticePosition(m_tetrahedraVertices.at(tetId)[0]);
-    auto v2 = this->getVerticePosition(m_tetrahedraVertices.at(tetId)[1]);
-    auto v3 = this->getVerticePosition(m_tetrahedraVertices.at(tetId)[2]);
-    auto v4 = this->getVerticePosition(m_tetrahedraVertices.at(tetId)[3]);
+    auto v1 = this->getVertexPosition(m_tetrahedraVertices.at(tetId)[0]);
+    auto v2 = this->getVertexPosition(m_tetrahedraVertices.at(tetId)[1]);
+    auto v3 = this->getVertexPosition(m_tetrahedraVertices.at(tetId)[2]);
+    auto v4 = this->getVertexPosition(m_tetrahedraVertices.at(tetId)[3]);
 
     std::array<double, 4> arrayx = { v1[0], v2[0], v3[0], v4[0] };
     std::array<double, 4> arrayy = { v1[1], v2[1], v3[1], v4[1] };
@@ -126,4 +126,10 @@ TetrahedralMesh::computeTetrahedronBoundingBox(const size_t& tetId, Vec3d& min,
     max[1] = *std::max_element(arrayy.begin(), arrayy.end());
     max[2] = *std::max_element(arrayz.begin(), arrayz.end());
 }
+
+bool TetrahedralMesh::extractSurfaceMesh(std::shared_ptr<TetrahedralMesh> tetMesh,
+                                         std::shared_ptr<SurfaceMesh> surfaceMesh)
+{
+}
+
 }

Base/Geometry/Mesh/imstkTetrahedralMesh.h

@@ -39,8 +39,20 @@ public:
     ~TetrahedralMesh() = default;
 
     // Accessors
+
+    ///
+    /// \brief set the vector of array of IDs for the mesh
+    ///
     void setTetrahedraVertices(const std::vector<TetraArray>& tetrahedrons);
+
+    ///
+    /// \brief Return the vector of array of IDs for all the tetrahedra
+    ///
     const std::vector<TetraArray>& getTetrahedraVertices() const;
+
+    ///
+    /// \brief Return the array of IDs for a given tetrahedron
+    ///
     const TetraArray& getTetrahedronVertices(const size_t& tetId) const;
 
     ///
@@ -63,6 +75,12 @@ public:
     ///
     void computeTetrahedronBoundingBox(const size_t& tetId, Vec3d& min, Vec3d& max) const;
 
+    ///
+    /// \brief Extract the confirming triangular mesh from the tetrahedral mesh
+    ///
+    static bool extractSurfaceMesh(std::shared_ptr<TetrahedralMesh> tetMesh,
+                                   std::shared_ptr<SurfaceMesh> surfaceMesh);
+
 protected:
 
     std::vector<TetraArray> m_tetrahedraVertices; ///< vertices of the tetrahedra