Commit db8f5442 authored by Sreekanth Arikatla's avatar Sreekanth Arikatla
Browse files

Merge branch 'PbdFunctors' into 'master'

REFAC: PbdConstraintFunctors and Containers

See merge request iMSTK/iMSTK!610
parents c2c902f2 d592a54b
......@@ -133,6 +133,4 @@ protected:
std::vector<Vec3d> m_dcdx;
};
using PBDConstraintVector = std::vector<std::shared_ptr<PbdConstraint>>;
}
}
\ No newline at end of file
/*=========================================================================
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::removeConstraints(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
/*=========================================================================
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 removeConstraints(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
......@@ -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
/*=========================================================================
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);
}