diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
index 419d13e9f8deae582803396707f3fb9f82a9c20f..0b54f9c1c7e83be85f54d2d7b1fdc83f63fe931e 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
@@ -133,6 +133,4 @@ protected:
std::vector<Vec3d> m_dcdx;
};
-
-using PBDConstraintVector = std::vector<std::shared_ptr<PbdConstraint>>;
-}
+}
\ No newline at end of file
diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstraintContainer.cpp b/Source/Constraint/PbdConstraints/imstkPbdConstraintContainer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..218071e7920c414bfcbab6c554fb1c3316d30992
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstraintContainer.cpp
@@ -0,0 +1,183 @@
+/*=========================================================================
+
+ 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 "imstkPbdConstraintContainer.h"
+#include "imstkGraph.h"
+
+#include <unordered_map>
+
+namespace imstk
+{
+void
+PbdConstraintContainer::addConstraint(std::shared_ptr<PbdConstraint> constraint)
+{
+ m_constraintLock.lock();
+ m_constraints.push_back(constraint);
+ m_constraintLock.unlock();
+}
+
+void
+PbdConstraintContainer::removeConstraint(std::shared_ptr<PbdConstraint> constraint)
+{
+ m_constraintLock.lock();
+ iterator i = std::find(m_constraints.begin(), m_constraints.end(), constraint);
+ if (i != m_constraints.end())
+ {
+ m_constraints.erase(i);
+ }
+ m_constraintLock.unlock();
+}
+
+void
+PbdConstraintContainer::removeConstraintVertices(std::shared_ptr<std::unordered_set<size_t>> vertices)
+{
+ // Remove constraints that contain the given vertices
+ auto removeConstraintFunc = [&](std::shared_ptr<PbdConstraint> constraint)
+ {
+ for (auto i : constraint->getVertexIds())
+ {
+ if (vertices->find(i) != vertices->end())
+ {
+ return true;
+ }
+ }
+ return false;
+ };
+
+ m_constraintLock.lock();
+ m_constraints.erase(std::remove_if(m_constraints.begin(), m_constraints.end(), removeConstraintFunc),
+ m_constraints.end());
+
+ // Also remove partitioned constraints
+ for (auto& pc : m_partitionedConstraints)
+ {
+ pc.erase(std::remove_if(pc.begin(), pc.end(), removeConstraintFunc), pc.end());
+ }
+
+ m_constraintLock.unlock();
+}
+
+void
+PbdConstraintContainer::eraseConstraint(iterator iter)
+{
+ m_constraintLock.lock();
+ m_constraints.erase(iter);
+ m_constraintLock.unlock();
+}
+
+void
+PbdConstraintContainer::partitionConstraints(const int partitionedThreshold)
+{
+ // Form the map { vertex : list_of_constraints_involve_vertex }
+ std::vector<std::shared_ptr<PbdConstraint>>& allConstraints = m_constraints;
+
+ //std::cout << "---------partitionConstraints: " << allConstraints.size() << std::endl;
+
+ std::unordered_map<size_t, std::vector<size_t>> vertexConstraints;
+ for (size_t constrIdx = 0; constrIdx < allConstraints.size(); ++constrIdx)
+ {
+ const auto& constr = allConstraints[constrIdx];
+ for (const auto& vIds : constr->getVertexIds())
+ {
+ vertexConstraints[vIds].push_back(constrIdx);
+ }
+ }
+
+ // Add edges to the constraint graph
+ // Each edge represent a shared vertex between two constraints
+ Graph constraintGraph(allConstraints.size());
+ for (const auto& kv : vertexConstraints)
+ {
+ const auto& constraints = kv.second; // the list of constraints for a vertex
+ for (size_t i = 0; i < constraints.size(); ++i)
+ {
+ for (size_t j = i + 1; j < constraints.size(); ++j)
+ {
+ constraintGraph.addEdge(constraints[i], constraints[j]);
+ }
+ }
+ }
+ vertexConstraints.clear();
+
+ // do graph coloring for the constraint graph
+ const auto coloring = constraintGraph.doColoring(Graph::ColoringMethod::WelshPowell);
+ // const auto coloring = constraintGraph.doColoring(Graph::ColoringMethod::Greedy);
+ const auto& partitionIndices = coloring.first;
+ const auto numPartitions = coloring.second;
+ assert(partitionIndices.size() == allConstraints.size());
+
+ std::vector<std::vector<std::shared_ptr<PbdConstraint>>>& partitionedConstraints = m_partitionedConstraints;
+ partitionedConstraints.resize(0);
+ partitionedConstraints.resize(static_cast<size_t>(numPartitions));
+
+ for (size_t constrIdx = 0; constrIdx < partitionIndices.size(); ++constrIdx)
+ {
+ const auto partitionIdx = partitionIndices[constrIdx];
+ partitionedConstraints[partitionIdx].push_back(allConstraints[constrIdx]);
+ }
+
+ // If a partition has size smaller than the partition threshold, then move its constraints back
+ // These constraints will be processed sequentially
+ // Because small size partitions yield bad performance upon running in parallel
+ allConstraints.resize(0);
+ for (const auto& constraints : partitionedConstraints)
+ {
+ if (constraints.size() < partitionedThreshold)
+ {
+ for (size_t constrIdx = 0; constrIdx < constraints.size(); ++constrIdx)
+ {
+ allConstraints.push_back(std::move(constraints[constrIdx]));
+ }
+ }
+ }
+
+ // Remove all empty partitions
+ size_t writeIdx = 0;
+ for (size_t readIdx = 0; readIdx < partitionedConstraints.size(); ++readIdx)
+ {
+ if (partitionedConstraints[readIdx].size() >= partitionedThreshold)
+ {
+ if (readIdx != writeIdx)
+ {
+ partitionedConstraints[writeIdx] = std::move(partitionedConstraints[readIdx]);
+ }
+ ++writeIdx;
+ }
+ }
+ partitionedConstraints.resize(writeIdx);
+
+ // Print
+ /*if (print)
+ {
+ size_t numConstraints = 0;
+ int idx = 0;
+ for (const auto& constraints : partitionedConstraints)
+ {
+ std::cout << "Partition # " << idx++ << " | # nodes: " << constraints.size() << std::endl;
+ numConstraints += constraints.size();
+ }
+ std::cout << "Sequential processing # nodes: " << allConstraints.size() << std::endl;
+ numConstraints += allConstraints.size();
+ std::cout << "Total constraints: " << numConstraints << " | Graph size: "
+ << constraintGraph.size() << std::endl;
+ }*/
+}
+}
\ No newline at end of file
diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstraintContainer.h b/Source/Constraint/PbdConstraints/imstkPbdConstraintContainer.h
new file mode 100644
index 0000000000000000000000000000000000000000..69e0165ccd88a4a51b512a5caca1fa312aef84b2
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstraintContainer.h
@@ -0,0 +1,103 @@
+/*=========================================================================
+
+ 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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkPbdConstraint.h"
+
+#include <unordered_set>
+
+namespace imstk
+{
+///
+/// \class PbdConstraintContainer
+///
+/// \brief Container for pbd constraints
+///
+class PbdConstraintContainer
+{
+public:
+ PbdConstraintContainer() = default;
+ virtual ~PbdConstraintContainer() = default;
+
+public:
+ using iterator = std::vector<std::shared_ptr<PbdConstraint>>::iterator;
+
+public:
+ ///
+ /// \brief Adds a constraint to the system, thread safe
+ ///
+ virtual void addConstraint(std::shared_ptr<PbdConstraint> constraint);
+
+ ///
+ /// \brief Linear searches for and removes a constraint from the system, thread safe
+ ///
+ virtual void removeConstraint(std::shared_ptr<PbdConstraint> constraint);
+
+ ///
+ /// \brief Removes all constraints associated with vertex ids
+ ///
+ virtual void removeConstraintVertices(std::shared_ptr<std::unordered_set<size_t>> vertices);
+
+ ///
+ /// \brief Removes a constraint from the system by iterator, thread safe
+ ///
+ virtual void eraseConstraint(iterator iter);
+
+ ///
+ /// \brief Reserve an amount of constraints in the pool, if you know
+ /// ahead of time the number of constraints, or even an estimate, it
+ /// can be faster to first reserve them
+ ///
+ virtual void reserve(const size_t n) { m_constraints.reserve(n); }
+
+ ///
+ /// \brief Returns if there are no constraints
+ ///
+ const bool empty() const { return m_constraints.empty() && m_partitionedConstraints.empty(); }
+
+ ///
+ /// \brief Get the underlying container
+ ///
+ const std::vector<std::shared_ptr<PbdConstraint>>& getConstraints() const { return m_constraints; }
+
+ ///
+ /// \brief Get the partitioned constraints
+ ///
+ const std::vector<std::vector<std::shared_ptr<PbdConstraint>>> getPartitionedConstraints() const { return m_partitionedConstraints; }
+
+ ///
+ /// \brief Partitions pbd constraints into separate vectors via graph coloring
+ /// \param Minimum number of constraints in groups, any under will be dumped back into m_constraints
+ ///
+ void partitionConstraints(const int partitionThreshold);
+
+ ///
+ /// \brief Clear the parition vectors
+ ///
+ void clearPartitions() { m_partitionedConstraints.clear(); }
+
+protected:
+ std::vector<std::shared_ptr<PbdConstraint>> m_constraints; ///> Not partitioned constraints
+ std::vector<std::vector<std::shared_ptr<PbdConstraint>>> m_partitionedConstraints; ///> Partitioned pbd constraints
+ ParallelUtils::SpinLock m_constraintLock; ///> Used to deal with concurrent addition/removal of constraints
+};
+}
\ No newline at end of file
diff --git a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
index b20802aacc8b05e8c31b942c8ba6ddd0c15ed91d..c1cc32169b8db46ca8f6e8878148a289a1656251 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
@@ -21,7 +21,7 @@
#include "imstkPbdVolumeConstraint.h"
-namespace imstk
+namespace imstk
{
void
PbdVolumeConstraint::initConstraint(const VecDataArray<double, 3>& initVertexPositions,
@@ -68,7 +68,8 @@ PbdVolumeConstraint::computeValueAndGradient(
dcdx[2] = onesixth * (x3 - x0).cross(x1 - x0);
dcdx[3] = onesixth * (x1 - x0).cross(x2 - x0);
- c = dcdx[3].dot(x3 - x0);
+ const double volume = dcdx[3].dot(x3 - x0);
+ c = (volume - m_restVolume);
return true;
}
} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdConstraintFunctor.h b/Source/DynamicalModels/ObjectModels/imstkPbdConstraintFunctor.h
new file mode 100644
index 0000000000000000000000000000000000000000..8733b8e4002d19e3d615bfdd58cf012b259964f3
--- /dev/null
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdConstraintFunctor.h
@@ -0,0 +1,419 @@
+/*=========================================================================
+
+ 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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkLineMesh.h"
+#include "imstkLogger.h"
+#include "imstkParallelUtils.h"
+#include "imstkPbdAreaConstraint.h"
+#include "imstkPbdBendConstraint.h"
+#include "imstkPbdConstantDensityConstraint.h"
+#include "imstkPbdConstraint.h"
+#include "imstkPbdDihedralConstraint.h"
+#include "imstkPbdDistanceConstraint.h"
+#include "imstkPbdFEMConstraint.h"
+#include "imstkPbdFETetConstraint.h"
+#include "imstkPbdVolumeConstraint.h"
+#include "imstkPointSet.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkTetrahedralMesh.h"
+#include "imstkPbdConstraintContainer.h"
+
+namespace imstk
+{
+///
+/// \brief PbdConstraintFunctor take input geometry and produce constraints
+///
+struct PbdConstraintFunctor
+{
+ public:
+ PbdConstraintFunctor() = default;
+ virtual ~PbdConstraintFunctor() = default;
+
+ ///
+ /// \brief Appends a set of constraint to the container given a geometry
+ ///
+ virtual void operator()(PbdConstraintContainer& constraints) = 0;
+
+ void setGeometry(std::shared_ptr<PointSet> geom) { m_geom = geom; }
+
+ public:
+ std::shared_ptr<PointSet> m_geom;
+};
+
+struct PbdDistanceConstraintFunctor : public PbdConstraintFunctor
+{
+ public:
+ PbdDistanceConstraintFunctor() = default;
+ ~PbdDistanceConstraintFunctor() override = default;
+
+ virtual void operator()(PbdConstraintContainer& constraints) override
+ {
+ const VecDataArray<double, 3>& vertices = *m_geom->getVertexPositions();
+ auto addDistConstraint =
+ [&](std::vector<std::vector<bool>>& E, size_t i1, size_t i2)
+ {
+ if (i1 > i2) // Make sure i1 is always smaller than i2
+ {
+ std::swap(i1, i2);
+ }
+ if (E[i1][i2])
+ {
+ E[i1][i2] = 0;
+ auto c = std::make_shared<PbdDistanceConstraint>();
+ c->initConstraint(vertices, i1, i2, m_stiffness);
+ constraints.addConstraint(c);
+ }
+ };
+
+ if (m_geom->getTypeName() == "TetrahedralMesh")
+ {
+ const auto& tetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_geom);
+ const VecDataArray<int, 4>& elements = *tetMesh->getTetrahedraIndices();
+ const auto nV = tetMesh->getNumVertices();
+ std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
+
+ for (int k = 0; k < elements.size(); ++k)
+ {
+ auto& tet = elements[k];
+ addDistConstraint(E, tet[0], tet[1]);
+ addDistConstraint(E, tet[0], tet[2]);
+ addDistConstraint(E, tet[0], tet[3]);
+ addDistConstraint(E, tet[1], tet[2]);
+ addDistConstraint(E, tet[1], tet[3]);
+ addDistConstraint(E, tet[2], tet[3]);
+ }
+ }
+ else if (m_geom->getTypeName() == "SurfaceMesh")
+ {
+ const auto& triMesh = std::dynamic_pointer_cast<SurfaceMesh>(m_geom);
+ const VecDataArray<int, 3>& elements = *triMesh->getTriangleIndices();
+ const auto nV = triMesh->getNumVertices();
+ std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
+
+ for (int k = 0; k < elements.size(); ++k)
+ {
+ auto& tri = elements[k];
+ addDistConstraint(E, tri[0], tri[1]);
+ addDistConstraint(E, tri[0], tri[2]);
+ addDistConstraint(E, tri[1], tri[2]);
+ }
+ }
+ else if (m_geom->getTypeName() == "LineMesh")
+ {
+ const auto& lineMesh = std::dynamic_pointer_cast<LineMesh>(m_geom);
+ const VecDataArray<int, 2>& elements = *lineMesh->getLinesIndices();
+ const auto& nV = lineMesh->getNumVertices();
+ std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
+
+ for (int k = 0; k < elements.size(); k++)
+ {
+ auto& seg = elements[k];
+ addDistConstraint(E, seg[0], seg[1]);
+ }
+ }
+ }
+
+ void setStiffness(double stiffness) { m_stiffness = stiffness; }
+
+ protected:
+ double m_stiffness;
+};
+
+struct PbdFemConstraintFunctor : public PbdConstraintFunctor
+{
+ public:
+ PbdFemConstraintFunctor() = default;
+ ~PbdFemConstraintFunctor() override = default;
+
+ virtual void operator()(PbdConstraintContainer& constraints) override
+ {
+ // Check if constraint type matches the mesh type
+ CHECK(m_geom->getTypeName() == "TetrahedralMesh")
+ << "FEM Tetrahedral constraint should come with tetrahedral mesh";
+
+ // Create constraints
+ auto tetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_geom);
+ const VecDataArray<double, 3>& vertices = *m_geom->getVertexPositions();
+ const VecDataArray<int, 4>& elements = *tetMesh->getTetrahedraIndices();
+
+ ParallelUtils::parallelFor(elements.size(),
+ [&](const size_t k)
+ {
+ const Vec4i& tet = elements[k];
+ auto c = std::make_shared<PbdFEMTetConstraint>(m_matType);
+ c->initConstraint(vertices,
+ tet[0], tet[1], tet[2], tet[3], m_femConfig);
+ constraints.addConstraint(c);
+ }, elements.size() > 100);
+ }
+
+ void setMaterialType(PbdFEMTetConstraint::MaterialType materialType) { m_matType = materialType; }
+ void setFemConfig(std::shared_ptr<PbdFEMConstraintConfig> femConfig) { m_femConfig = femConfig; }
+
+ protected:
+ PbdFEMTetConstraint::MaterialType m_matType;
+ std::shared_ptr<PbdFEMConstraintConfig> m_femConfig;
+};
+
+struct PbdVolumeConstraintFunctor : public PbdConstraintFunctor
+{
+ public:
+ PbdVolumeConstraintFunctor() = default;
+ ~PbdVolumeConstraintFunctor() override = default;
+
+ virtual void operator()(PbdConstraintContainer& constraints) override
+ {
+ // Check if constraint type matches the mesh type
+ CHECK(m_geom->getTypeName() == "TetrahedralMesh")
+ << "Volume constraint should come with volumetric mesh";
+
+ // Create constraints
+ auto tetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_geom);
+ const VecDataArray<double, 3>& vertices = *m_geom->getVertexPositions();
+ const VecDataArray<int, 4>& elements = *tetMesh->getTetrahedraIndices();
+
+ ParallelUtils::parallelFor(elements.size(),
+ [&](const size_t k)
+ {
+ auto& tet = elements[k];
+ auto c = std::make_shared<PbdVolumeConstraint>();
+ c->initConstraint(vertices,
+ tet[0], tet[1], tet[2], tet[3], m_stiffness);
+ constraints.addConstraint(c);
+ });
+ }
+
+ void setStiffness(double stiffness) { m_stiffness = stiffness; }
+
+ protected:
+ double m_stiffness;
+};
+
+struct PbdAreaConstraintFunctor : public PbdConstraintFunctor
+{
+ public:
+ PbdAreaConstraintFunctor() = default;
+ ~PbdAreaConstraintFunctor() override = default;
+
+ virtual void operator()(PbdConstraintContainer& constraints) override
+ {
+ // check if constraint type matches the mesh type
+ CHECK(m_geom->getTypeName() == "SurfaceMesh")
+ << "Area constraint should come with a triangular mesh";
+
+ // ok, now create constraints
+ auto triMesh = std::dynamic_pointer_cast<SurfaceMesh>(m_geom);
+ const VecDataArray<double, 3>& vertices = *m_geom->getVertexPositions();
+ const VecDataArray<int, 3>& elements = *triMesh->getTriangleIndices();
+
+ ParallelUtils::parallelFor(elements.size(),
+ [&](const size_t k)
+ {
+ auto& tri = elements[k];
+ auto c = std::make_shared<PbdAreaConstraint>();
+ c->initConstraint(vertices, tri[0], tri[1], tri[2], m_stiffness);
+ constraints.addConstraint(c);
+ });
+ }
+
+ void setStiffness(double stiffness) { m_stiffness = stiffness; }
+
+ protected:
+ double m_stiffness;
+};
+
+struct PbdBendConstraintFunctor : public PbdConstraintFunctor
+{
+ public:
+ PbdBendConstraintFunctor() = default;
+ ~PbdBendConstraintFunctor() override = default;
+
+ virtual void operator()(PbdConstraintContainer& constraints) override
+ {
+ CHECK(m_geom->getTypeName() == "LineMesh")
+ << "Bend constraint should come with a line mesh";
+
+ auto lineMesh = std::dynamic_pointer_cast<LineMesh>(m_geom);
+ const VecDataArray<double, 3>& vertices = *m_geom->getVertexPositions();
+ const VecDataArray<int, 2>& elements = *lineMesh->getLinesIndices();
+
+ auto addBendConstraint =
+ [&](const double k, size_t i1, size_t i2, size_t i3)
+ {
+ // i1 should always come first
+ if (i2 < i1)
+ {
+ std::swap(i1, i2);
+ }
+ // i3 should always come last
+ if (i2 > i3)
+ {
+ std::swap(i2, i3);
+ }
+
+ auto c = std::make_shared<PbdBendConstraint>();
+ c->initConstraint(vertices, i1, i2, i3, k);
+ constraints.addConstraint(c);
+ };
+
+ // Iterate sets of two segments
+ for (int k = 0; k < elements.size() - 1; k++)
+ {
+ auto& seg1 = elements[k];
+ auto& seg2 = elements[k + 1];
+ int i3 = seg2[0];
+ if (i3 == seg1[0] || i3 == seg1[1])
+ {
+ i3 = seg2[1];
+ }
+ addBendConstraint(m_stiffness, seg1[0], seg1[1], i3);
+ }
+ }
+
+ void setStiffness(double stiffness) { m_stiffness = stiffness; }
+
+ protected:
+ double m_stiffness;
+};
+
+struct PbdDihedralConstraintFunctor : public PbdConstraintFunctor
+{
+ public:
+ PbdDihedralConstraintFunctor() = default;
+ ~PbdDihedralConstraintFunctor() override = default;
+
+ virtual void operator()(PbdConstraintContainer& constraints) override
+ {
+ CHECK(m_geom->getTypeName() == "SurfaceMesh")
+ << "Dihedral constraint should come with a triangular mesh";
+
+ // Create constraints
+ auto triMesh = std::dynamic_pointer_cast<SurfaceMesh>(m_geom);
+ const VecDataArray<double, 3>& vertices = *triMesh->getVertexPositions();
+ const VecDataArray<int, 3>& elements = *triMesh->getTriangleIndices();
+ const int nV = triMesh->getNumVertices();
+ std::vector<std::vector<int>> vertIdsToTriangleIds(nV);
+
+ for (int k = 0; k < elements.size(); ++k)
+ {
+ const Vec3i& tri = elements[k];
+ vertIdsToTriangleIds[tri[0]].push_back(k);
+ vertIdsToTriangleIds[tri[1]].push_back(k);
+ vertIdsToTriangleIds[tri[2]].push_back(k);
+ }
+
+ // Used to resolve duplicates
+ std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
+
+ auto addDihedralConstraint =
+ [&](const std::vector<int>& r1, const std::vector<int>& r2,
+ const int k, int i1, int i2)
+ {
+ if (i1 > i2) // Make sure i1 is always smaller than i2
+ {
+ std::swap(i1, i2);
+ }
+ if (E[i1][i2])
+ {
+ E[i1][i2] = 0;
+
+ // Find the shared edge
+ std::vector<size_t> rs(2);
+ auto it = std::set_intersection(r1.begin(), r1.end(), r2.begin(), r2.end(), rs.begin());
+ rs.resize(static_cast<size_t>(it - rs.begin()));
+ if (rs.size() > 1)
+ {
+ size_t idx = (rs[0] == k) ? 1 : 0;
+ const auto& tri0 = elements[k];
+ const auto& tri1 = elements[rs[idx]];
+ size_t idx0 = 0;
+ size_t idx1 = 0;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ if (tri0[i] != i1 && tri0[i] != i2)
+ {
+ idx0 = tri0[i];
+ }
+ if (tri1[i] != i1 && tri1[i] != i2)
+ {
+ idx1 = tri1[i];
+ }
+ }
+ auto c = std::make_shared<PbdDihedralConstraint>();
+ c->initConstraint(vertices, idx0, idx1, i1, i2, m_stiffness);
+ constraints.addConstraint(c);
+ }
+ }
+ };
+
+ for (int i = 0; i < vertIdsToTriangleIds.size(); i++)
+ {
+ std::sort(vertIdsToTriangleIds[i].begin(), vertIdsToTriangleIds[i].end());
+ }
+
+ // For every triangle
+ for (int k = 0; k < elements.size(); ++k)
+ {
+ const Vec3i& tri = elements[k];
+
+ // Get all the neighbor triangles (to the vertices)
+ std::vector<int>& neighborTriangles0 = vertIdsToTriangleIds[tri[0]];
+ std::vector<int>& neighborTriangles1 = vertIdsToTriangleIds[tri[1]];
+ std::vector<int>& neighborTriangles2 = vertIdsToTriangleIds[tri[2]];
+
+ // Add constraints between all the triangles
+ addDihedralConstraint(neighborTriangles0, neighborTriangles1, k, tri[0], tri[1]);
+ addDihedralConstraint(neighborTriangles0, neighborTriangles2, k, tri[0], tri[2]);
+ addDihedralConstraint(neighborTriangles1, neighborTriangles2, k, tri[1], tri[2]);
+ }
+ }
+
+ void setStiffness(double stiffness) { m_stiffness = stiffness; }
+
+ protected:
+ double m_stiffness;
+};
+
+struct PbdConstantDensityConstraintFunctor : public PbdConstraintFunctor
+{
+ public:
+ PbdConstantDensityConstraintFunctor() = default;
+ ~PbdConstantDensityConstraintFunctor() override = default;
+
+ virtual void operator()(PbdConstraintContainer& constraints) override
+ {
+ // check if constraint type matches the mesh type
+ CHECK(std::dynamic_pointer_cast<PointSet>(m_geom) != nullptr)
+ << "Constant constraint should come with a mesh!";
+
+ auto c = std::make_shared<PbdConstantDensityConstraint>();
+ c->initConstraint(*m_geom->getVertexPositions(), m_stiffness);
+ constraints.addConstraint(c);
+ }
+
+ void setStiffness(double stiffness) { m_stiffness = stiffness; }
+
+ protected:
+ double m_stiffness;
+};
+}
\ No newline at end of file
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp b/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
index 8986b73e01551b01ee4a133171a285f2ac547160..56a6f09debaace5c5e01cf3eef52d9b377e6ad34 100644
--- a/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
@@ -24,17 +24,12 @@
#include "imstkLineMesh.h"
#include "imstkLogger.h"
#include "imstkParallelUtils.h"
-#include "imstkPbdAreaConstraint.h"
-#include "imstkPbdBendConstraint.h"
-#include "imstkPbdConstantDensityConstraint.h"
-#include "imstkPbdDihedralConstraint.h"
-#include "imstkPbdDistanceConstraint.h"
#include "imstkPbdFETetConstraint.h"
#include "imstkPbdSolver.h"
-#include "imstkPbdVolumeConstraint.h"
#include "imstkSurfaceMesh.h"
#include "imstkTaskGraph.h"
#include "imstkTetrahedralMesh.h"
+#include "imstkPbdConstraintFunctor.h"
#include <map>
#include <set>
@@ -110,74 +105,73 @@ PbdModel::initialize()
// Initialize constraints
{
- m_constraints = std::make_shared<PBDConstraintVector>();
- m_partitionedConstraints = std::make_shared<std::vector<PBDConstraintVector>>();
+ m_constraints = std::make_shared<PbdConstraintContainer>();
- // Initialize FEM constraints
- for (auto& constraint : m_parameters->m_FEMConstraints)
+ computeElasticConstants();
+
+ auto pointSet = std::dynamic_pointer_cast<PointSet>(getModelGeometry());
+ for (auto constraintType : m_parameters->m_FEMConstraints)
{
- computeElasticConstants();
- if (!initializeFEMConstraints(constraint.second))
- {
- return false;
- }
+ auto functor = std::make_shared<PbdFemConstraintFunctor>();
+ functor->setFemConfig(m_parameters->m_femParams);
+ functor->setMaterialType(constraintType.second);
+ m_functors.push_back(functor);
}
-
- // Initialize other constraints
- for (auto& constraint : m_parameters->m_regularConstraints)
+ for (auto constraintType : m_parameters->m_regularConstraints)
{
- if (m_parameters->m_solverType == PbdConstraint::SolverType::PBD && constraint.second > 1.0)
+ if (constraintType.first == PbdConstraint::Type::Area)
{
- LOG(WARNING) << "for PBD, k should be between [0, 1]";
+ auto functor = std::make_shared<PbdAreaConstraintFunctor>();
+ functor->setStiffness(constraintType.second);
+ m_functors.push_back(functor);
}
- else if (m_parameters->m_solverType == PbdConstraint::SolverType::xPBD && constraint.second <= 1.0)
+ else if (constraintType.first == PbdConstraint::Type::Bend)
{
- LOG(WARNING) << "for xPBD, k is Young's Modulu, and should be much larger than 1";
+ auto functor = std::make_shared<PbdBendConstraintFunctor>();
+ functor->setStiffness(constraintType.second);
+ m_functors.push_back(functor);
}
-
- if (!bOK)
+ else if (constraintType.first == PbdConstraint::Type::ConstantDensity)
{
- return false;
+ auto functor = std::make_shared<PbdConstantDensityConstraintFunctor>();
+ functor->setStiffness(constraintType.second);
+ m_functors.push_back(functor);
}
- switch (constraint.first)
+ else if (constraintType.first == PbdConstraint::Type::Dihedral)
{
- case PbdConstraint::Type::Volume:
- bOK = initializeVolumeConstraints(constraint.second);
- break;
-
- case PbdConstraint::Type::Distance:
- bOK = initializeDistanceConstraints(constraint.second);
- break;
-
- case PbdConstraint::Type::Area:
- bOK = initializeAreaConstraints(constraint.second);
- break;
-
- case PbdConstraint::Type::Bend:
- bOK = initializeBendConstraints(constraint.second);
- break;
-
- case PbdConstraint::Type::Dihedral:
- bOK = initializeDihedralConstraints(constraint.second);
- break;
-
- case PbdConstraint::Type::ConstantDensity:
- bOK = initializeConstantDensityConstraint(constraint.second);
- break;
-
- default:
- LOG(FATAL) << "Invalid constraint type";
+ auto functor = std::make_shared<PbdDihedralConstraintFunctor>();
+ functor->setStiffness(constraintType.second);
+ m_functors.push_back(functor);
+ }
+ else if (constraintType.first == PbdConstraint::Type::Distance)
+ {
+ auto functor = std::make_shared<PbdDistanceConstraintFunctor>();
+ functor->setStiffness(constraintType.second);
+ m_functors.push_back(functor);
+ }
+ else if (constraintType.first == PbdConstraint::Type::Volume)
+ {
+ auto functor = std::make_shared<PbdVolumeConstraintFunctor>();
+ functor->setStiffness(constraintType.second);
+ m_functors.push_back(functor);
}
}
+ for (auto functorPtr : m_functors)
+ {
+ PbdConstraintFunctor& functor = *functorPtr;
+ functor.setGeometry(pointSet);
+ functor(*m_constraints);
+ }
+
// Partition constraints for parallel computation
if (m_parameters->m_doPartitioning)
{
- this->partitionConstraints();
+ m_constraints->partitionConstraints(m_partitionThreshold);
}
else
{
- m_partitionedConstraints->clear();
+ m_constraints->clearPartitions();
}
}
@@ -191,7 +185,6 @@ PbdModel::initialize()
m_pbdSolver->setPositions(getCurrentState()->getPositions());
m_pbdSolver->setInvMasses(getInvMasses());
m_pbdSolver->setConstraints(getConstraints());
- m_pbdSolver->setPartitionedConstraints(getPartitionedConstraints());
m_pbdSolver->setTimeStep(m_parameters->m_dt);
this->setTimeStepSizeType(m_timeStepSizeType);
@@ -301,326 +294,11 @@ PbdModel::computeElasticConstants()
}
}
-bool
-PbdModel::initializeFEMConstraints(PbdFEMConstraint::MaterialType type)
-{
- // Check if constraint type matches the mesh type
- CHECK(m_mesh->getTypeName() == "TetrahedralMesh")
- << "FEM Tetrahedral constraint should come with tetrahedral mesh";
-
- // Create constraints
- const auto& tetMesh = std::static_pointer_cast<TetrahedralMesh>(m_mesh);
- const VecDataArray<int, 4>& elements = *tetMesh->getTetrahedraIndices();
-
- ParallelUtils::SpinLock lock;
- ParallelUtils::parallelFor(elements.size(),
- [&](const size_t k)
- {
- const Vec4i& tet = elements[k];
- auto c = std::make_shared<PbdFEMTetConstraint>(type);
- c->initConstraint(*m_initialState->getPositions(),
- tet[0], tet[1], tet[2], tet[3], m_parameters->m_femParams);
- lock.lock();
- m_constraints->push_back(std::move(c));
- lock.unlock();
- }, elements.size() > 100);
- return true;
-}
-
-bool
-PbdModel::initializeVolumeConstraints(const double stiffness)
-{
- // Check if constraint type matches the mesh type
- CHECK(m_mesh->getTypeName() == "TetrahedralMesh")
- << "Volume constraint should come with volumetric mesh";
-
- // Create constraints
- const auto& tetMesh = std::static_pointer_cast<TetrahedralMesh>(m_mesh);
- const VecDataArray<int, 4>& elements = *tetMesh->getTetrahedraIndices();
-
- ParallelUtils::SpinLock lock;
- ParallelUtils::parallelFor(elements.size(),
- [&](const size_t k)
- {
- auto& tet = elements[k];
- auto c = std::make_shared<PbdVolumeConstraint>();
- c->initConstraint(*m_initialState->getPositions(),
- tet[0], tet[1], tet[2], tet[3], stiffness);
- lock.lock();
- m_constraints->push_back(std::move(c));
- lock.unlock();
- });
- return true;
-}
-
-bool
-PbdModel::initializeDistanceConstraints(const double stiffness)
-{
- auto addConstraint =
- [&](std::vector<std::vector<bool>>& E, size_t i1, size_t i2)
- {
- if (i1 > i2) // Make sure i1 is always smaller than i2
- {
- std::swap(i1, i2);
- }
- if (E[i1][i2])
- {
- E[i1][i2] = 0;
- auto c = std::make_shared<PbdDistanceConstraint>();
- c->initConstraint(*m_initialState->getPositions(), i1, i2, stiffness);
- m_constraints->push_back(std::move(c));
- }
- };
-
- if (m_mesh->getTypeName() == "TetrahedralMesh")
- {
- const auto& tetMesh = std::static_pointer_cast<TetrahedralMesh>(m_mesh);
- const VecDataArray<int, 4>& elements = *tetMesh->getTetrahedraIndices();
- const auto nV = tetMesh->getNumVertices();
- std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
-
- for (int k = 0; k < elements.size(); ++k)
- {
- auto& tet = elements[k];
- addConstraint(E, tet[0], tet[1]);
- addConstraint(E, tet[0], tet[2]);
- addConstraint(E, tet[0], tet[3]);
- addConstraint(E, tet[1], tet[2]);
- addConstraint(E, tet[1], tet[3]);
- addConstraint(E, tet[2], tet[3]);
- }
- }
- else if (m_mesh->getTypeName() == "SurfaceMesh")
- {
- const auto& triMesh = std::static_pointer_cast<SurfaceMesh>(m_mesh);
- const VecDataArray<int, 3>& elements = *triMesh->getTriangleIndices();
- const auto nV = triMesh->getNumVertices();
- std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
-
- for (int k = 0; k < elements.size(); ++k)
- {
- auto& tri = elements[k];
- addConstraint(E, tri[0], tri[1]);
- addConstraint(E, tri[0], tri[2]);
- addConstraint(E, tri[1], tri[2]);
- }
- }
- else if (m_mesh->getTypeName() == "LineMesh")
- {
- const auto& lineMesh = std::static_pointer_cast<LineMesh>(m_mesh);
- const VecDataArray<int, 2>& elements = *lineMesh->getLinesIndices();
- const auto& nV = lineMesh->getNumVertices();
- std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
-
- for (int k = 0; k < elements.size(); k++)
- {
- auto& seg = elements[k];
- addConstraint(E, seg[0], seg[1]);
- }
- }
-
- return true;
-}
-
-bool
-PbdModel::initializeAreaConstraints(const double stiffness)
-{
- // check if constraint type matches the mesh type
- CHECK(m_mesh->getTypeName() == "SurfaceMesh")
- << "Area constraint should come with a triangular mesh";
-
- // ok, now create constraints
- const auto& triMesh = std::static_pointer_cast<SurfaceMesh>(m_mesh);
- const VecDataArray<int, 3>& elements = *triMesh->getTriangleIndices();
-
- ParallelUtils::SpinLock lock;
- ParallelUtils::parallelFor(elements.size(),
- [&](const size_t k)
- {
- auto& tri = elements[k];
- auto c = std::make_shared<PbdAreaConstraint>();
- c->initConstraint(*m_initialState->getPositions(), tri[0], tri[1], tri[2], stiffness);
- lock.lock();
- m_constraints->push_back(std::move(c));
- lock.unlock();
- });
- return true;
-}
-
-bool
-PbdModel::initializeBendConstraints(const double stiffness)
-{
- CHECK(m_mesh->getTypeName() == "LineMesh")
- << "Bend constraint should come with a line mesh";
-
- auto addConstraint =
- [&](const double k, size_t i1, size_t i2, size_t i3)
- {
- // i1 should always come first
- if (i2 < i1)
- {
- std::swap(i1, i2);
- }
- // i3 should always come last
- if (i2 > i3)
- {
- std::swap(i2, i3);
- }
-
- auto c = std::make_shared<PbdBendConstraint>();
- c->initConstraint(*m_initialState->getPositions(), i1, i2, i3, k);
- m_constraints->push_back(std::move(c));
- };
-
- // Create constraints
- const auto& lineMesh = std::static_pointer_cast<LineMesh>(m_mesh);
- const VecDataArray<int, 2>& elements = *lineMesh->getLinesIndices();
-
- // Iterate sets of two segments
- for (int k = 0; k < elements.size() - 1; k++)
- {
- auto& seg1 = elements[k];
- auto& seg2 = elements[k + 1];
- int i3 = seg2[0];
- if (i3 == seg1[0] || i3 == seg1[1])
- {
- i3 = seg2[1];
- }
- addConstraint(stiffness, seg1[0], seg1[1], i3);
- }
- return true;
-}
-
-bool
-PbdModel::initializeDihedralConstraints(const double stiffness)
-{
- CHECK(m_mesh->getTypeName() == "SurfaceMesh")
- << "Dihedral constraint should come with a triangular mesh";
-
- // Create constraints
- const auto& triMesh = std::static_pointer_cast<SurfaceMesh>(m_mesh);
- const VecDataArray<int, 3>& elements = *triMesh->getTriangleIndices();
- const auto nV = triMesh->getNumVertices();
- std::vector<std::vector<int>> vertIdsToTriangleIds(nV);
-
- for (int k = 0; k < elements.size(); ++k)
- {
- const Vec3i& tri = elements[k];
- vertIdsToTriangleIds[tri[0]].push_back(k);
- vertIdsToTriangleIds[tri[1]].push_back(k);
- vertIdsToTriangleIds[tri[2]].push_back(k);
- }
-
- // Used to resolve duplicates
- std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
-
- auto addConstraint =
- [&](const std::vector<int>& r1, const std::vector<int>& r2,
- const int k, int i1, int i2)
- {
- if (i1 > i2) // Make sure i1 is always smaller than i2
- {
- std::swap(i1, i2);
- }
- if (E[i1][i2])
- {
- E[i1][i2] = 0;
-
- // Find the shared edge
- std::vector<size_t> rs(2);
- auto it = std::set_intersection(r1.begin(), r1.end(), r2.begin(), r2.end(), rs.begin());
- rs.resize(static_cast<size_t>(it - rs.begin()));
- if (rs.size() > 1)
- {
- size_t idx = (rs[0] == k) ? 1 : 0;
- const auto& tri0 = elements[k];
- const auto& tri1 = elements[rs[idx]];
- size_t idx0 = 0;
- size_t idx1 = 0;
- for (size_t i = 0; i < 3; ++i)
- {
- if (tri0[i] != i1 && tri0[i] != i2)
- {
- idx0 = tri0[i];
- }
- if (tri1[i] != i1 && tri1[i] != i2)
- {
- idx1 = tri1[i];
- }
- }
- auto c = std::make_shared<PbdDihedralConstraint>();
- c->initConstraint(*m_initialState->getPositions(), idx0, idx1, i1, i2, stiffness);
- m_constraints->push_back(std::move(c));
- }
- }
- };
-
- for (int i = 0; i < vertIdsToTriangleIds.size(); i++)
- {
- std::sort(vertIdsToTriangleIds[i].begin(), vertIdsToTriangleIds[i].end());
- }
-
- // For every triangle
- for (int k = 0; k < elements.size(); ++k)
- {
- const Vec3i& tri = elements[k];
-
- // Get all the neighbor triangles (to the vertices)
- std::vector<int>& neighborTriangles0 = vertIdsToTriangleIds[tri[0]];
- std::vector<int>& neighborTriangles1 = vertIdsToTriangleIds[tri[1]];
- std::vector<int>& neighborTriangles2 = vertIdsToTriangleIds[tri[2]];
-
- // Add constraints between all the triangles
- addConstraint(neighborTriangles0, neighborTriangles1, k, tri[0], tri[1]);
- addConstraint(neighborTriangles0, neighborTriangles2, k, tri[0], tri[2]);
- addConstraint(neighborTriangles1, neighborTriangles2, k, tri[1], tri[2]);
- }
- return true;
-}
-
-bool
-PbdModel::initializeConstantDensityConstraint(const double stiffness)
-{
- // check if constraint type matches the mesh type
- CHECK(std::dynamic_pointer_cast<PointSet>(m_mesh) != nullptr)
- << "Constant constraint should come with a mesh!";
-
- auto c = std::make_shared<PbdConstantDensityConstraint>();
- c->initConstraint(*m_initialState->getPositions(), stiffness);
- m_constraints->push_back(std::move(c));
-
- return true;
-}
-
-void
-PbdModel::removeConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
-{
- // constraint removal predicate
- auto removeConstraint =
- [&](std::shared_ptr<PbdConstraint> constraint)
- {
- for (auto i : constraint->getVertexIds())
- {
- if (vertices->find(i) != vertices->end())
- {
- return true;
- }
- }
- return false;
- };
-
- // remove constraints from constraint-partition maps and constraint vectors.
- m_constraints->erase(std::remove_if(m_constraints->begin(), m_constraints->end(), removeConstraint),
- m_constraints->end());
- for (auto& pc : *m_partitionedConstraints)
- {
- pc.erase(std::remove_if(pc.begin(), pc.end(), removeConstraint), pc.end());
- }
-}
-
void
PbdModel::addConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
{
+ // \todo: Refactor into functors, then move to ConstrainerContainer
+
// check if constraint type matches the mesh type
CHECK(m_mesh->getTypeName() == "SurfaceMesh")
<< "Add element constraints does not support current mesh type.";
@@ -653,7 +331,7 @@ PbdModel::addConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
{
auto c = std::make_shared<PbdDistanceConstraint>();
c->initConstraint(*m_initialState->getPositions(), i1, i2, stiffness);
- m_constraints->push_back(std::move(c));
+ m_constraints->addConstraint(c);
};
auto addAreaConstraint =
[&](size_t k, double stiffness)
@@ -661,7 +339,7 @@ PbdModel::addConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
auto& tri = (*elements)[k];
auto c = std::make_shared<PbdAreaConstraint>();
c->initConstraint(*m_initialState->getPositions(), tri[0], tri[1], tri[2], stiffness);
- m_constraints->push_back(std::move(c));
+ m_constraints->addConstraint(c);
};
auto addDihedralConstraint =
[&](size_t t0, size_t t1, size_t i1, size_t i2, double stiffness)
@@ -683,7 +361,7 @@ PbdModel::addConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
}
auto c = std::make_shared<PbdDihedralConstraint>();
c->initConstraint(*m_initialState->getPositions(), idx0, idx1, i1, i2, stiffness);
- m_constraints->push_back(std::move(c));
+ m_constraints->addConstraint(c);
};
// count constraints to be added for pre-allocation
@@ -761,7 +439,7 @@ PbdModel::addConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
}
// add constraints
- m_constraints->reserve(m_constraints->size() + distanceSet.size() + areaSet.size() + dihedralSet.size());
+ m_constraints->reserve(m_constraints->getConstraints().size() + distanceSet.size() + areaSet.size() + dihedralSet.size());
for (auto& constraint : m_parameters->m_regularConstraints)
{
switch (constraint.first)
@@ -785,116 +463,6 @@ PbdModel::addConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
}
}
-void
-PbdModel::addConstraint(std::shared_ptr<PbdConstraint> constraint)
-{
- m_constraints->push_back(constraint);
-}
-
-void
-PbdModel::removeConstraint(std::shared_ptr<PbdConstraint> constraint)
-{
- m_constraints->erase(std::find(m_constraints->begin(), m_constraints->end(), constraint));
-}
-
-void
-PbdModel::partitionConstraints(const bool print)
-{
- // Form the map { vertex : list_of_constraints_involve_vertex }
- PBDConstraintVector& allConstraints = *m_constraints;
-
- //std::cout << "---------partitionConstraints: " << allConstraints.size() << std::endl;
-
- std::unordered_map<size_t, std::vector<size_t>> vertexConstraints;
- for (size_t constrIdx = 0; constrIdx < allConstraints.size(); ++constrIdx)
- {
- const auto& constr = allConstraints[constrIdx];
- for (const auto& vIds : constr->getVertexIds())
- {
- vertexConstraints[vIds].push_back(constrIdx);
- }
- }
-
- // Add edges to the constraint graph
- // Each edge represent a shared vertex between two constraints
- Graph constraintGraph(allConstraints.size());
- for (const auto& kv : vertexConstraints)
- {
- const auto& constraints = kv.second; // the list of constraints for a vertex
- for (size_t i = 0; i < constraints.size(); ++i)
- {
- for (size_t j = i + 1; j < constraints.size(); ++j)
- {
- constraintGraph.addEdge(constraints[i], constraints[j]);
- }
- }
- }
- vertexConstraints.clear();
-
- // do graph coloring for the constraint graph
- const auto coloring = constraintGraph.doColoring(Graph::ColoringMethod::WelshPowell);
- // const auto coloring = constraintGraph.doColoring(Graph::ColoringMethod::Greedy);
- const auto& partitionIndices = coloring.first;
- const auto numPartitions = coloring.second;
- assert(partitionIndices.size() == allConstraints.size());
-
- std::vector<PBDConstraintVector>& partitionedConstraints = *m_partitionedConstraints;
- partitionedConstraints.resize(0);
- partitionedConstraints.resize(static_cast<size_t>(numPartitions));
-
- for (size_t constrIdx = 0; constrIdx < partitionIndices.size(); ++constrIdx)
- {
- const auto partitionIdx = partitionIndices[constrIdx];
- partitionedConstraints[partitionIdx].push_back(allConstraints[constrIdx]);
- }
-
- // If a partition has size smaller than the partition threshold, then move its constraints back
- // These constraints will be processed sequentially
- // Because small size partitions yield bad performance upon running in parallel
- allConstraints.resize(0);
- for (const auto& constraints : partitionedConstraints)
- {
- if (constraints.size() < m_partitionThreshold)
- {
- for (size_t constrIdx = 0; constrIdx < constraints.size(); ++constrIdx)
- {
- allConstraints.push_back(std::move(constraints[constrIdx]));
- }
- }
- }
-
- // Remove all empty partitions
- size_t writeIdx = 0;
- for (size_t readIdx = 0; readIdx < partitionedConstraints.size(); ++readIdx)
- {
- if (partitionedConstraints[readIdx].size() >= m_partitionThreshold)
- {
- if (readIdx != writeIdx)
- {
- partitionedConstraints[writeIdx] = std::move(partitionedConstraints[readIdx]);
- }
- ++writeIdx;
- }
- }
- partitionedConstraints.resize(writeIdx);
-
- // Print
- if (print)
- {
- size_t numConstraints = 0;
- int idx = 0;
- for (const auto& constraints : partitionedConstraints)
- {
- std::cout << "Partition # " << idx++ << " | # nodes: " << constraints.size() << std::endl;
- numConstraints += constraints.size();
- }
- std::cout << "Sequential processing # nodes: " << allConstraints.size() << std::endl;
- numConstraints += allConstraints.size();
- std::cout << "Total constraints: " << numConstraints << " | Graph size: "
- << constraintGraph.size() << std::endl;
- }
-}
-
void
PbdModel::setParticleMass(const double val, const size_t idx)
{
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdModel.h b/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
index 3098532583a85d27b281c20a7c29e6ad8f114242..d73d67b6806845496ac14f6c1d9813c658ccdbb7 100644
--- a/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
@@ -31,7 +31,9 @@
namespace imstk
{
+struct PbdConstraintFunctor;
class PointSet;
+class PbdConstraintContainer;
class PbdSolver;
///
@@ -124,80 +126,20 @@ public:
///
void computeElasticConstants();
- ///
- /// \brief Initialize FEM constraints
- ///
- bool initializeFEMConstraints(PbdFEMConstraint::MaterialType type);
-
- ///
- /// \brief Initialize volume constraints
- ///
- bool initializeVolumeConstraints(const double stiffness);
-
- ///
- /// \brief Initialize distance constraints
- ///
- bool initializeDistanceConstraints(const double stiffness);
-
- ///
- /// \brief Initialize area constraints
- ///
- bool initializeAreaConstraints(const double stiffness);
-
- ///
- /// \brief Initialize bend constraints
- ///
- bool initializeBendConstraints(const double stiffness);
-
- ///
- /// \brief Initialize dihedral constraints
- ///
- bool initializeDihedralConstraints(const double stiffness);
-
- ///
- /// \brief Initialize constant density constraints for PBD fluid
- ///
- bool initializeConstantDensityConstraint(const double stiffness);
-
- ///
- /// \brief Returns true if there is at least one constraint
- ///
- bool hasConstraints() const { return !m_constraints->empty() || !m_partitionedConstraints->empty(); }
-
- ///
- /// \brief Remove constraints related to a set of vertices.
- ///
- void removeConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices);
-
///
/// \brief Add constraints related to a set of vertices.
/// \brief Does not check for duplicating pre-existed constraints.
+ /// \todo: Move to containers and functors
///
void addConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices);
- ///
- /// \brief Adds a constraint to the system, added to the set of sequentially
- /// solved constraints
- ///
- void addConstraint(std::shared_ptr<PbdConstraint> constraint);
-
- ///
- /// \brief Searches for and removes a constraint from the system
- ///
- void removeConstraint(std::shared_ptr<PbdConstraint> constraint);
-
virtual void setTimeStep(const double timeStep) override { m_parameters->m_dt = timeStep; }
double getTimeStep() const override { return m_parameters->m_dt; }
///
/// \brief Return all constraints that are solved sequentially
///
- std::shared_ptr<PBDConstraintVector> getConstraints() { return m_constraints; }
-
- ///
- /// \brief Return the constraints that are colored and run in parallel
- ///
- std::shared_ptr<std::vector<PBDConstraintVector>> getPartitionedConstraints() { return m_partitionedConstraints; }
+ std::shared_ptr<PbdConstraintContainer> getConstraints() { return m_constraints; }
///
/// \brief Set mass to particular node
@@ -259,6 +201,11 @@ public:
///
void setSolver(std::shared_ptr<PbdSolver> solver) { this->m_pbdSolver = solver; }
+ ///
+ /// \brief Adds a functor to generate constraints
+ ///
+ void addPbdConstraintFunctor(std::shared_ptr<PbdConstraintFunctor> functor) { m_functors.push_back(functor); }
+
std::shared_ptr<TaskNode> getIntegratePositionNode() const { return m_integrationPositionNode; }
std::shared_ptr<TaskNode> getUpdateCollisionGeometryNode() const { return m_updateCollisionGeometryNode; }
@@ -268,11 +215,6 @@ public:
std::shared_ptr<TaskNode> getUpdateVelocityNode() const { return m_updateVelocityNode; }
protected:
- ///
- /// \brief Partition constraints for parallelization
- ///
- void partitionConstraints(const bool print = false);
-
///
/// \brief Setup the computational graph of PBD
///
@@ -287,10 +229,12 @@ protected:
std::shared_ptr<DataArray<double>> m_invMass = nullptr; ///> Inverse of mass of nodes
std::shared_ptr<std::unordered_map<size_t, double>> m_fixedNodeInvMass = nullptr; ///> Map for archiving fixed nodes' mass.
- std::shared_ptr<PBDConstraintVector> m_constraints = nullptr; ///> List of pbd constraints
- std::shared_ptr<std::vector<PBDConstraintVector>> m_partitionedConstraints = nullptr; ///> List of pbd constraints
std::shared_ptr<PBDModelConfig> m_parameters = nullptr; ///> Model parameters, must be set before simulation
+protected:
+ std::shared_ptr<PbdConstraintContainer> m_constraints; ///> The set of constraints to update/use
+ std::vector<std::shared_ptr<PbdConstraintFunctor>> m_functors; ///> The set of functors for constraint generation
+
protected:
// Computational Nodes
std::shared_ptr<TaskNode> m_integrationPositionNode = nullptr;
diff --git a/Source/Scene/imstkPbdObjectCuttingPair.cpp b/Source/Scene/imstkPbdObjectCuttingPair.cpp
index 12b04c31f21ccff1856ea857ba604a28a7917dca..04aa449bc31d7eefb7bf0dc880a38ac08c02da8b 100644
--- a/Source/Scene/imstkPbdObjectCuttingPair.cpp
+++ b/Source/Scene/imstkPbdObjectCuttingPair.cpp
@@ -25,6 +25,7 @@ limitations under the License.
#include "imstkCollidingObject.h"
#include "imstkLogger.h"
#include "imstkNew.h"
+#include "imstkPbdConstraintContainer.h"
#include "imstkPbdModel.h"
#include "imstkPbdObject.h"
#include "imstkPbdSolver.h"
@@ -82,7 +83,8 @@ PbdObjectCuttingPair::apply()
// update pbd states, constraints and solver
pbdModel->initState();
- pbdModel->removeConstraints(m_removeConstraintVertices);
+ pbdModel->getConstraints()->removeConstraintVertices(m_removeConstraintVertices);
+ // pbdModel->getConstraints()->addConstraintVertices(m_addConstraintVertices);
pbdModel->addConstraints(m_addConstraintVertices);
pbdModel->getSolver()->setInvMasses(pbdModel->getInvMasses());
pbdModel->getSolver()->setPositions(pbdModel->getCurrentState()->getPositions());
diff --git a/Source/Solvers/imstkPbdSolver.cpp b/Source/Solvers/imstkPbdSolver.cpp
index 285184c1fc249e1b6452d34219126c5486eabe27..d10db83aec61c41ffe239e8c706efd619f68e918 100644
--- a/Source/Solvers/imstkPbdSolver.cpp
+++ b/Source/Solvers/imstkPbdSolver.cpp
@@ -23,13 +23,13 @@
#include "imstkLogger.h"
#include "imstkParallelUtils.h"
#include "imstkPbdCollisionConstraint.h"
+#include "imstkPbdConstraintContainer.h"
namespace imstk
{
PbdSolver::PbdSolver() :
m_dt(0.0),
- m_partitionedConstraints(std::make_shared<std::vector<PBDConstraintVector>>()),
- m_constraints(std::make_shared<PBDConstraintVector>()),
+ m_constraints(std::make_shared<PbdConstraintContainer>()),
m_positions(std::make_shared<VecDataArray<double, 3>>()),
m_invMasses(std::make_shared<DataArray<double>>())
{
@@ -55,8 +55,8 @@ PbdSolver::solve()
VecDataArray<double, 3>& currPositions = *m_positions;
const DataArray<double>& invMasses = *m_invMasses;
- const PBDConstraintVector& constraints = *m_constraints;
- const std::vector<PBDConstraintVector>& partitionedConstraints = *m_partitionedConstraints;
+ const std::vector<std::shared_ptr<PbdConstraint>>& constraints = m_constraints->getConstraints();
+ const std::vector<std::vector<std::shared_ptr<PbdConstraint>>>& partitionedConstraints = m_constraints->getPartitionedConstraints();
// zero out the Lagrange multiplier
for (size_t j = 0; j < constraints.size(); ++j)
@@ -66,7 +66,7 @@ PbdSolver::solve()
for (size_t j = 0; j < partitionedConstraints.size(); j++)
{
- const PBDConstraintVector& constraintPartition = partitionedConstraints[j];
+ const std::vector<std::shared_ptr<PbdConstraint>>& constraintPartition = partitionedConstraints[j];
ParallelUtils::parallelFor(constraintPartition.size(),
[&](const size_t idx) { constraintPartition[idx]->zeroOutLambda(); }
);
@@ -82,7 +82,7 @@ PbdSolver::solve()
for (size_t j = 0; j < partitionedConstraints.size(); j++)
{
- const PBDConstraintVector& constraintPartition = partitionedConstraints[j];
+ const std::vector<std::shared_ptr<PbdConstraint>>& constraintPartition = partitionedConstraints[j];
ParallelUtils::parallelFor(constraintPartition.size(),
[&](const size_t idx)
diff --git a/Source/Solvers/imstkPbdSolver.h b/Source/Solvers/imstkPbdSolver.h
index 2e76d48a3931ba5228f840aa63c58059d98dd59a..e50a119626a5351aa73c5ac3ba233c4675c84931 100644
--- a/Source/Solvers/imstkPbdSolver.h
+++ b/Source/Solvers/imstkPbdSolver.h
@@ -27,6 +27,7 @@
namespace imstk
{
class PbdCollisionConstraint;
+class PbdConstraintContainer;
///
/// \struct CollisionConstraintData
@@ -70,17 +71,11 @@ public:
///
void setIterations(const size_t iterations) { this->m_iterations = iterations; }
- ///
- /// \brief Sets the partioned constraints the solver should solve for
- /// These will be solved in parallel
- ///
- void setPartitionedConstraints(std::shared_ptr<std::vector<PBDConstraintVector>> partitionedConstraints) { this->m_partitionedConstraints = partitionedConstraints; }
-
///
/// \brief Sets the constraints the solver should solve for
/// These wil be solved sequentially
///
- void setConstraints(std::shared_ptr<PBDConstraintVector> constraints) { this->m_constraints = constraints; }
+ void setConstraints(std::shared_ptr<PbdConstraintContainer> constraints) { this->m_constraints = constraints; }
///
/// \brief Sets the positions the solver should solve with
@@ -113,11 +108,10 @@ public:
void solve() override;
private:
- size_t m_iterations = 20; ///> Number of NL Gauss-Seidel iterations for regular constraints
- double m_dt; ///> time step
+ size_t m_iterations = 20; ///> Number of NL Gauss-Seidel iterations for regular constraints
+ double m_dt; ///> time step
- std::shared_ptr<std::vector<PBDConstraintVector>> m_partitionedConstraints = nullptr; ///> Set of vector'd/partitioned pbd constraints
- std::shared_ptr<PBDConstraintVector> m_constraints = nullptr; ///> Vector of constraints
+ std::shared_ptr<PbdConstraintContainer> m_constraints = nullptr; ///> Vector of constraints
std::shared_ptr<VecDataArray<double, 3>> m_positions = nullptr;
std::shared_ptr<DataArray<double>> m_invMasses = nullptr;