diff --git a/Base/Geometry/imstkCube.cpp b/Base/Geometry/imstkCube.cpp
index 1971889ca05361701e420d8cdec7dfbec553ca6c..afa243e58bbdb7e1343ff7db128a8dcaa39fef2b 100644
--- a/Base/Geometry/imstkCube.cpp
+++ b/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;
+}
+
}
diff --git a/Base/Geometry/imstkCube.h b/Base/Geometry/imstkCube.h
index 34336c9a0e6b01fd8b74bddebab57e1b47a1aa20..60dacfdc38e855b52f45dd7ae94af7941aa0ebbc 100644
--- a/Base/Geometry/imstkCube.h
+++ b/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;
diff --git a/Base/Geometry/imstkGeometry.cpp b/Base/Geometry/imstkGeometry.cpp
index f2b190378d4db4bc884916840cc3db709e9fc0e0..1a55e84a4e19cd8afbaad0739b8a83cab34c1900 100644
--- a/Base/Geometry/imstkGeometry.cpp
+++ b/Base/Geometry/imstkGeometry.cpp
@@ -22,6 +22,7 @@
#include "imstkGeometry.h"
namespace imstk {
+
void
Geometry::translate(const Vec3d& t)
{
diff --git a/Base/Geometry/imstkGeometry.h b/Base/Geometry/imstkGeometry.h
index bfc81ca1d2a3130defacbb2ab23953b1e57a2d58..63f8941b51d6e79fe73ceee6920cf92b1a97463c 100644
--- a/Base/Geometry/imstkGeometry.h
+++ b/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,
diff --git a/Base/Geometry/imstkGeometryMap.cpp b/Base/Geometry/imstkGeometryMap.cpp
index ebc60e13d9bc60bd1d3440e46d91f6a369c53cb6..8dfc9d891f788e8b3f0e688624909b28bb89cb49 100644
--- a/Base/Geometry/imstkGeometryMap.cpp
+++ b/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
diff --git a/Base/Geometry/imstkGeometryMap.h b/Base/Geometry/imstkGeometryMap.h
index 335bb5807354c1fdf01bf2f052c780a17f7d771e..134f3e58a2103ce4b8325f39289106721b498718 100644
--- a/Base/Geometry/imstkGeometryMap.h
+++ b/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.
diff --git a/Base/Geometry/imstkHexahedralMesh.cpp b/Base/Geometry/imstkHexahedralMesh.cpp
index 86ba2fd506bf5ebc65aa4e9412743dc5487d00ce..16f252d5769b1802df5c5c140bd7d51fbf2998a2 100644
--- a/Base/Geometry/imstkHexahedralMesh.cpp
+++ b/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;
+}
+
}
diff --git a/Base/Geometry/imstkHexahedralMesh.h b/Base/Geometry/imstkHexahedralMesh.h
index dea35888dd7f79aeed36cc02e7da61e9436d7481..9a29b65a76be7929e80c23c0d1a54ffbfb584c18 100644
--- a/Base/Geometry/imstkHexahedralMesh.h
+++ b/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:
diff --git a/Base/Geometry/imstkIdentityMap.cpp b/Base/Geometry/imstkIdentityMap.cpp
index 18afe8b9239b927035a51a901ddb58775c5a7e4e..fd3e6155c4cf20932755ae5190546a315061795a 100644
--- a/Base/Geometry/imstkIdentityMap.cpp
+++ b/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
diff --git a/Base/Geometry/imstkIsometricMap.cpp b/Base/Geometry/imstkIsometricMap.cpp
index 2663f0ef6510913bd612c0f335d716776fd8ba83..c8a67a862a2d72b9a789ddfd462fd6bb271e1316 100644
--- a/Base/Geometry/imstkIsometricMap.cpp
+++ b/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
diff --git a/Base/Geometry/imstkMesh.cpp b/Base/Geometry/imstkMesh.cpp
index 441246e4f6997b9f2bec93bce48ecad2c9fdb7bf..7c76a302afc520030ffade0c7a40e52d375e32fd 100644
--- a/Base/Geometry/imstkMesh.cpp
+++ b/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);
+ }
+}
+
}
diff --git a/Base/Geometry/imstkMesh.h b/Base/Geometry/imstkMesh.h
index 43e7011213bae18540725ef7ffa08188999b9677..378361cb72c09de9c6249670af82d2d45d66058a 100644
--- a/Base/Geometry/imstkMesh.h
+++ b/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()) {}
diff --git a/Base/Geometry/imstkPlane.cpp b/Base/Geometry/imstkPlane.cpp
index 7093d14ca310bc82e5a853be0f0eeb2847ddb2cf..1f1625a72bee181f7711376b5b33c8fc6359a8ae 100644
--- a/Base/Geometry/imstkPlane.cpp
+++ b/Base/Geometry/imstkPlane.cpp
@@ -45,4 +45,10 @@ Plane::setWidth(const double& width)
{
m_width = width;
}
+
+double
+Plane::getVolume() const
+{
+ return 0.0;
+}
}
diff --git a/Base/Geometry/imstkPlane.h b/Base/Geometry/imstkPlane.h
index 0acb5c941acf6e04a72fd16f351f01e7c55c4dab..e0395e93eaad5e6674bec8b9b36991c54a66003f 100644
--- a/Base/Geometry/imstkPlane.h
+++ b/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;
diff --git a/Base/Geometry/imstkSphere.cpp b/Base/Geometry/imstkSphere.cpp
index edfa76ec0ff596941ce53e2af47166b74c8c29c6..86780f482db6bd670eef1b75c60db8baccaa109e 100644
--- a/Base/Geometry/imstkSphere.cpp
+++ b/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;
+}
+
}
diff --git a/Base/Geometry/imstkSphere.h b/Base/Geometry/imstkSphere.h
index d07c9a3b4335445cca15268ebed18db47238729d..96d5c7bfafbae12cace4a2fd7a7722bc58d4f189 100644
--- a/Base/Geometry/imstkSphere.h
+++ b/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;
diff --git a/Base/Geometry/imstkSurfaceMesh.cpp b/Base/Geometry/imstkSurfaceMesh.cpp
index c309a2588018e5502fd4c0f1bbac91dd1fff3024..15e765457c9295a40f32bb46129feacfdbfd6171 100644
--- a/Base/Geometry/imstkSurfaceMesh.cpp
+++ b/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;
+}
+
}
diff --git a/Base/Geometry/imstkSurfaceMesh.h b/Base/Geometry/imstkSurfaceMesh.h
index 2cfab16c0ec833d53975d46db8d9cdfd2f445031..abe286dc5f1d3f7ed57bbae9de37968bd4556190 100644
--- a/Base/Geometry/imstkSurfaceMesh.h
+++ b/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;
diff --git a/Base/Geometry/imstkTetraTriangleMap.cpp b/Base/Geometry/imstkTetraTriangleMap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd4068e8aad9a34c4d162b42424cb27138996e4a
--- /dev/null
+++ b/Base/Geometry/imstkTetraTriangleMap.cpp
@@ -0,0 +1,182 @@
+/*=========================================================================
+
+ 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);
+ }
+
+ // compute the weights
+ tetMesh->computeBarycentricWeights(closestEle, surfVertPos, weights);
+
+ m_enclosingTetra.push_back(closestEle);// store nearest tetrahedra
+ m_weights.push_back(weights);// store weights
+ }
+}
+
+int
+TetraTriangleMap::findClosestTetrahedra(const std::shared_ptr<imstk::TetrahedralMesh> tetraMesh, const imstk::Vec3d& p)
+{
+ // search
+ double closestDistance = std::numeric_limits<double>::max();
+ int closestTetrahedra = -1;
+ for (int t = 0; t < tetraMesh->getNumTetrahedra(); ++t)
+ {
+ imstk::Vec3d center(0, 0, 0);
+ auto vert = tetraMesh->getTetrahedronVertices(t);
+ for (int i = 0; i < 4; ++i)
+ {
+ center += tetraMesh->getInitialVertexPosition(vert[i]);
+ }
+
+ double dist = (p - center).norm();
+ if (dist < closestDistance)
+ {
+ closestDistance = dist;
+ closestTetrahedra = t;
+ }
+ }
+
+ return closestTetrahedra;
+}
+
+int
+TetraTriangleMap::findEclosingTetrahedra(const std::shared_ptr<imstk::TetrahedralMesh> tetraMesh, const imstk::Vec3d& p)
+{
+ imstk::Vec3d boundingBoxMin;
+ imstk::Vec3d boundingBoxMax;
+ std::vector<int> probables;
+
+ // Eliminate the improbables based in bounding box test
+ for (int t = 0; t < tetraMesh->getNumTetrahedra(); ++t)
+ {
+ tetraMesh->computeTetrahedraBoundingBox(boundingBoxMin, boundingBoxMax, t);
+
+ if ((p[0] >= boundingBoxMin[0] && p[0] <= boundingBoxMax[0]) &&
+ (p[1] >= boundingBoxMin[1] && p[1] <= boundingBoxMax[1]) &&
+ (p[2] >= boundingBoxMin[2] && p[2] <= boundingBoxMax[2]))
+ {
+ probables.push_back(t);
+ }
+ }
+
+ // Check which probable tetrahedra the point belongs to
+ int elclosingTetra = -1;
+ weightsArray weights;
+ for (auto it = probables.begin(); it != probables.end(); ++it)
+ {
+ tetraMesh->computeBarycentricWeights(*it, p, weights);
+
+ if ((weights[0] >= 0) && (weights[1] >= 0) && (weights[2] >= 0) && (weights[3] >= 0))
+ {
+ elclosingTetra = *it;
+ break;
+ }
+ }
+ return elclosingTetra;
+}
+
+void
+TetraTriangleMap::setMaster(std::shared_ptr<Geometry> master)
+{
+ if (master->getType() == imstk::GeometryType::TetrahedralMesh)
+ {
+ m_master = master;
+ }
+ else
+ {
+ LOG(WARNING) << "The geometry provided is not of tetrahedral type\n";
+ }
+}
+
+void
+TetraTriangleMap::setSlave(std::shared_ptr<Geometry> slave)
+{
+ if (slave->getType() == imstk::GeometryType::SurfaceMesh)
+ {
+ m_slave = slave;
+ }
+ else
+ {
+ LOG(WARNING) << "The geometry provided is not of surface triangular type\n";
+ }
+
+}
+
+void
+TetraTriangleMap::applyMap()
+{
+ if (m_isActive)
+ {
+ if (!m_master || !m_slave)
+ {
+ LOG(WARNING) << "TetraTriangle map is not completely defined!\n";
+ return;
+ }
+
+ auto tetMesh = std::dynamic_pointer_cast<imstk::TetrahedralMesh> (m_master);
+ auto triMesh = std::dynamic_pointer_cast<imstk::SurfaceMesh> (m_slave);
+
+ imstk::Vec3d newPos;
+ for (int v = 0; v < triMesh->getNumVertices(); ++v)
+ {
+ newPos.setZero();
+ auto tetVerts = tetMesh->getTetrahedronVertices(m_enclosingTetra[v]);
+ for (int i = 0; i < 4; ++i)
+ {
+ newPos += tetMesh->getInitialVertexPosition(tetVerts[i]) * m_weights.at(v)[i];
+ }
+ triMesh->setVertexPosition(v, newPos);
+ }
+ }
+}
+}
\ No newline at end of file
diff --git a/Base/Geometry/imstkTetraTriangleMap.h b/Base/Geometry/imstkTetraTriangleMap.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d1e1203fab7f959666e415807233cc0acc00d0b
--- /dev/null
+++ b/Base/Geometry/imstkTetraTriangleMap.h
@@ -0,0 +1,89 @@
+/*=========================================================================
+
+ 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 imstkTetraTriangleMap_h
+#define imstkTetraTriangleMap_h
+
+#include <limits>
+
+#include "imstkGeometryMap.h"
+#include "imstkTetrahedralMesh.h"
+#include "imstkSurfaceMesh.h"
+
+namespace imstk {
+
+///
+/// \class TetraTriangleMap
+///
+/// \brief Computes and applies the triangle-tetrahedra map. The master mesh is the
+/// tetrahedral mesh and the slave is the surface triangular mesh.
+///
+class TetraTriangleMap : public GeometryMap
+{
+ using weightsArray = std::array<double, 4>;
+
+public:
+
+ ~TetraTriangleMap() = default;
+
+ TetraTriangleMap() : GeometryMap(GeometryMapType::TetraTriangle){}
+
+ ///
+ /// \brief Apply (if active) the tetra-triangle mesh map
+ ///
+ void applyMap();
+
+ ///
+ /// \brief Compute the tetra-triangle mesh map
+ ///
+ void computeMap();
+
+ // Generic utility functions
+
+ ///
+ /// \brief Find the closest tetrahedra based on the distance to their centroids for a given point in 3D space
+ ///
+ static int findClosestTetrahedra(const std::shared_ptr<imstk::TetrahedralMesh> tetraMesh, const imstk::Vec3d& p);
+
+ ///
+ /// \brief Find the tetrahedra that encloses a given point in 3D space
+ ///
+ static int findEclosingTetrahedra(const std::shared_ptr<imstk::TetrahedralMesh> tetraMesh, const imstk::Vec3d& p);
+
+ // Accessors
+
+ ///
+ /// \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::vector<weightsArray> m_weights; ///> weights
+ std::vector<int> m_enclosingTetra; ///> Enclosing tetrahedra to interpolate the weights upon
+};
+}
+
+#endif // imstkTetraTriangleMap_h
diff --git a/Base/Geometry/imstkTetrahedralMesh.cpp b/Base/Geometry/imstkTetrahedralMesh.cpp
index eee4dca4f9796a252afc5758f7d6d052e607d27f..bed460a41609770d15d623dc640ba473670aa36f 100644
--- a/Base/Geometry/imstkTetrahedralMesh.cpp
+++ b/Base/Geometry/imstkTetrahedralMesh.cpp
@@ -28,9 +28,103 @@ TetrahedralMesh::getTetrahedronVertices() const
return m_tetrahedronVertices;
}
+const imstk::TetrahedralMesh::TetraArray& TetrahedralMesh::getTetrahedronVertices(const int tetraNum) const
+{
+ return m_tetrahedronVertices.at(tetraNum);
+}
+
void
TetrahedralMesh::setTetrahedronVertices(const std::vector<TetraArray>& tetrahedrons)
{
m_tetrahedronVertices = tetrahedrons;
}
+
+void
+TetrahedralMesh::computeBarycentricWeights(const int closestEle, const imstk::Vec3d& p, std::array<double, 4> weights) const
+{
+ TetraArray vertIndices = getTetrahedronVertices(closestEle);
+ imstk::Vec3d v[4];
+ double dets[4];
+ double det;
+
+ for (int i = 0; i < 4; i++)
+ {
+ v[i] = getVertexPosition(vertIndices[i]);
+ }
+
+ Eigen::Matrix4d A;
+ A << v[0][0], v[0][1], v[0][2], 1,
+ v[1][0], v[1][1], v[1][2], 1,
+ v[2][0], v[2][1], v[2][2], 1,
+ v[3][0], v[3][1], v[3][2], 1;
+
+ det = A.determinant();
+
+ for (int i = 0; i < 4; i++)
+ {
+ Eigen::Matrix4d B = A;
+ B(i, 0) = p[0];
+ B(i, 1) = p[1];
+ B(i, 2) = p[2];
+ weights[i] = B.determinant() / det;
+ }
+}
+
+void
+TetrahedralMesh::computeTetrahedraBoundingBox(imstk::Vec3d& min, imstk::Vec3d& max, const int tetNum) const
+{
+ auto v1 = getVertexPosition(m_tetrahedronVertices.at(tetNum)[0]);
+ auto v2 = getVertexPosition(m_tetrahedronVertices.at(tetNum)[1]);
+ auto v3 = getVertexPosition(m_tetrahedronVertices.at(tetNum)[2]);
+ auto v4 = getVertexPosition(m_tetrahedronVertices.at(tetNum)[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] };
+ std::array<double, 4> arrayz = { v1[2], v2[2], v3[2], v4[2] };
+
+ min[0] = *std::min_element(arrayx.begin(), arrayx.end());
+ min[1] = *std::min_element(arrayy.begin(), arrayy.end());
+ min[2] = *std::min_element(arrayz.begin(), arrayz.end());
+
+ max[0] = *std::max_element(arrayx.begin(), arrayx.end());
+ max[1] = *std::max_element(arrayy.begin(), arrayy.end());
+ max[2] = *std::max_element(arrayz.begin(), arrayz.end());
+}
+
+double
+TetrahedralMesh::getVolume() const
+{
+ imstk::Vec3d v[4];
+ Eigen::Matrix4d A;
+ double volume = 0.0;
+ for (auto it = m_tetrahedronVertices.begin(); it != m_tetrahedronVertices.end(); ++it)
+ {
+ for (int i = 0; i < 4; i++)
+ {
+ v[i] = getVertexPosition((*it)[i]);
+ }
+
+ A << v[0][0], v[0][1], v[0][2], 1,
+ v[1][0], v[1][1], v[1][2], 1,
+ v[2][0], v[2][1], v[2][2], 1,
+ v[3][0], v[3][1], v[3][2], 1;
+
+ double det = A.determinant();
+ if (det < 0)
+ {
+ LOG(WARNING) << "Tetrahedon is inverted, has negative volume!\n";
+ }
+
+ volume += std::abs(det)/6;
+ }
+
+ return volume;
+}
+
+int
+TetrahedralMesh::getNumTetrahedra() const
+{
+ return m_tetrahedronVertices.size();
+}
+
}
diff --git a/Base/Geometry/imstkTetrahedralMesh.h b/Base/Geometry/imstkTetrahedralMesh.h
index 84f3ad4981e3bab03c2abfdb44f07b9a5436adab..d548e9d5ff0600d684607cb68e23047896e7657e 100644
--- a/Base/Geometry/imstkTetrahedralMesh.h
+++ b/Base/Geometry/imstkTetrahedralMesh.h
@@ -39,8 +39,32 @@ public:
// Accessors
const std::vector<TetraArray>& getTetrahedronVertices() const;
- void setTetrahedronVertices(
- const std::vector<TetraArray>& tetrahedrons);
+ void setTetrahedronVertices(const std::vector<TetraArray>& tetrahedrons);
+
+ ///
+ /// \brief Returns the number of tetrahedra
+ ///
+ int getNumTetrahedra() const;
+
+ ///
+ /// \brief compute the barycentric weights of a given point in 3D space for a given the tetrahedra
+ ///
+ void computeBarycentricWeights(const int closestEle, const imstk::Vec3d& p, std::array<double, 4> weights) const;
+
+ ///
+ /// \brief get the indices vertices of a given tetrahedra in an array
+ ///
+ const TetraArray& getTetrahedronVertices(const int tetraNum) const;
+
+ ///
+ /// \brief Compute the bounding box of a given tetrahedra
+ ///
+ void computeTetrahedraBoundingBox(imstk::Vec3d& min, imstk::Vec3d& max, const int tetNum) const;
+
+ ///
+ /// \brief Compute and return the volume of the tetrahedral mesh
+ ///
+ double getVolume() const;
protected:
diff --git a/Examples/Sandbox/main.cpp b/Examples/Sandbox/main.cpp
index 9e979ea24f227bb03d3e042694621a5c92607e16..d33a5b07a11a8854904a4f021bec5856c1b24760 100644
--- a/Examples/Sandbox/main.cpp
+++ b/Examples/Sandbox/main.cpp
@@ -30,7 +30,7 @@ int main()
testViewer();
//testAnalyticalGeometry();
//testScenesManagement();
- //testGeometryMaps();
+ //testIsometricMaps();
return 0;
}
@@ -178,7 +178,15 @@ void testScenesManagement()
while (sdk->getStatus() != imstk::SimulationStatus::INACTIVE) {}
}
-void testGeometryMaps()
+void testTetraTriangleMap()
+{
+ // SDK and Scene
+ auto sdk = std::make_shared<imstk::SimulationManager>();
+ auto geometryMapTest = sdk->createNewScene("geometryMapTest");
+ geometryMapTest->setLoopDelay(1000);
+}
+
+void testIsometricMaps()
{
// SDK and Scene
auto sdk = std::make_shared<imstk::SimulationManager>();