From 6b79f2a37c0cf423370191c2a89bdbc14f65ead0 Mon Sep 17 00:00:00 2001
From: Sreekanth Arikatla <sreekanth.arikatla@kitware.com>
Date: Mon, 11 Apr 2016 17:39:28 -0400
Subject: [PATCH] WIP: Adds Tetra-Triangular mesh map
Adds Tetra-Triangular mesh map.
TODO: Code check and add tests
---
Base/Geometry/imstkCube.cpp | 8 ++
Base/Geometry/imstkCube.h | 1 +
Base/Geometry/imstkGeometry.cpp | 1 +
Base/Geometry/imstkGeometry.h | 3 +
Base/Geometry/imstkGeometryMap.cpp | 87 ++++++-----
Base/Geometry/imstkGeometryMap.h | 4 +-
Base/Geometry/imstkHexahedralMesh.cpp | 66 ++++++++-
Base/Geometry/imstkHexahedralMesh.h | 12 +-
Base/Geometry/imstkIdentityMap.cpp | 35 ++---
Base/Geometry/imstkIsometricMap.cpp | 51 ++++---
Base/Geometry/imstkMesh.cpp | 57 ++++++++
Base/Geometry/imstkMesh.h | 12 ++
Base/Geometry/imstkPlane.cpp | 6 +
Base/Geometry/imstkPlane.h | 1 +
Base/Geometry/imstkSphere.cpp | 7 +
Base/Geometry/imstkSphere.h | 1 +
Base/Geometry/imstkSurfaceMesh.cpp | 23 +++
Base/Geometry/imstkSurfaceMesh.h | 4 +
Base/Geometry/imstkTetraTriangleMap.cpp | 182 ++++++++++++++++++++++++
Base/Geometry/imstkTetraTriangleMap.h | 89 ++++++++++++
Base/Geometry/imstkTetrahedralMesh.cpp | 94 ++++++++++++
Base/Geometry/imstkTetrahedralMesh.h | 28 +++-
Examples/Sandbox/main.cpp | 12 +-
23 files changed, 696 insertions(+), 88 deletions(-)
create mode 100644 Base/Geometry/imstkTetraTriangleMap.cpp
create mode 100644 Base/Geometry/imstkTetraTriangleMap.h
diff --git a/Base/Geometry/imstkCube.cpp b/Base/Geometry/imstkCube.cpp
index 1971889ca..afa243e58 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 34336c9a0..60dacfdc3 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 f2b190378..1a55e84a4 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 bfc81ca1d..63f8941b5 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 ebc60e13d..8dfc9d891 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 335bb5807..134f3e58a 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 86ba2fd50..16f252d57 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 dea35888d..9a29b65a7 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 18afe8b92..fd3e6155c 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 2663f0ef6..c8a67a862 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 441246e4f..7c76a302a 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 43e701121..378361cb7 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 7093d14ca..1f1625a72 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 0acb5c941..e0395e93e 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 edfa76ec0..86780f482 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 d07c9a3b4..96d5c7bfa 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 c309a2588..15e765457 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 2cfab16c0..abe286dc5 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 000000000..dd4068e8a
--- /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 000000000..8d1e1203f
--- /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 eee4dca4f..bed460a41 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 84f3ad498..d548e9d5f 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 9e979ea24..d33a5b07a 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>();
--
GitLab