diff --git a/Examples/MultipleScenes/multipleScenes.cpp b/Examples/MultipleScenes/multipleScenes.cpp
index e120419ee22a37d9f17b185365812f075b0f0217..8b5cd383bc0ab66fc08555a8da941cc08f20ffaf 100644
--- a/Examples/MultipleScenes/multipleScenes.cpp
+++ b/Examples/MultipleScenes/multipleScenes.cpp
@@ -65,8 +65,8 @@ createSoftBodyScene(std::shared_ptr<SimulationManager> simManager, const char* s
auto pbdParams = std::make_shared<PBDModelConfig>();
// FEM constraint
- pbdParams->femParams->m_YoungModulus = 100.0;
- pbdParams->femParams->m_PoissonRatio = 0.3;
+ pbdParams->m_femParams->m_YoungModulus = 100.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.3;
pbdParams->m_fixedNodeIds = { 51, 127, 178 };
pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet, PbdFEMConstraint::MaterialType::StVK);
diff --git a/Examples/PBD/PBDCloth/pbdClothExample.cpp b/Examples/PBD/PBDCloth/pbdClothExample.cpp
index c76b56a558a1d6fb254781d2d0962a357bd3663c..bfbcc57d17e1dc1521cc5f8dc9bc7043584041e5 100644
--- a/Examples/PBD/PBDCloth/pbdClothExample.cpp
+++ b/Examples/PBD/PBDCloth/pbdClothExample.cpp
@@ -99,17 +99,16 @@ main()
auto pbdParams = std::make_shared<PBDModelConfig>();
// Constraints
- pbdParams->enableConstraint(PbdConstraint::Type::Distance, 0.1);
- pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 0.001);
- std::vector<size_t> fixedNodes(nCols);
- for (size_t i = 0; i < fixedNodes.size(); i++)
- {
- fixedNodes[i] = i;
- }
+ // pbdParams->enableConstraint(PbdConstraint::Type::Distance, 0.1);
+ // pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 1e2);
+ // pbdParams->m_solverType = PbdConstraint::SolverType::PBD;
+ pbdParams->enableConstraint(PbdConstraint::Type::Distance, 1e2);
+ pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 1e1);
+ std::vector<size_t> fixedNodes = { 0, nCols - 1 };
pbdParams->m_fixedNodeIds = fixedNodes;
// Other parameters
- pbdParams->m_uniformMassValue = 1.0;
+ pbdParams->m_uniformMassValue = width * height / (nRows * nCols);
pbdParams->m_gravity = Vec3d(0, -9.8, 0);
pbdParams->m_defaultDt = 0.005;
pbdParams->m_iterations = 5;
diff --git a/Examples/PBD/PBDCollisionMultipleObjects/PBDCollisionMultipleObjectsExample.cpp b/Examples/PBD/PBDCollisionMultipleObjects/PBDCollisionMultipleObjectsExample.cpp
index 3011315c23af84fe2b9e46340bcb0301380b2fbe..ed1e6083bdab7216fe910523c411c34c906dbf94 100644
--- a/Examples/PBD/PBDCollisionMultipleObjects/PBDCollisionMultipleObjectsExample.cpp
+++ b/Examples/PBD/PBDCollisionMultipleObjects/PBDCollisionMultipleObjectsExample.cpp
@@ -120,8 +120,8 @@ generateDragon(const std::shared_ptr<imstk::Scene>& scene,
auto pbdParams = std::make_shared<PBDModelConfig>();
// FEM constraint
- pbdParams->femParams->m_YoungModulus = 1000.0;
- pbdParams->femParams->m_PoissonRatio = 0.3;
+ pbdParams->m_femParams->m_YoungModulus = 1000.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.3;
pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet, PbdFEMConstraint::MaterialType::StVK);
// Other parameters
diff --git a/Examples/PBD/PBDCollisionOneObject/PBDCollisionOneObjectExample.cpp b/Examples/PBD/PBDCollisionOneObject/PBDCollisionOneObjectExample.cpp
index 0e39f97bf68c5321d47426158c6865ddd86af3b5..31f6e7ff62684e4333c08366386da79f5310a338 100644
--- a/Examples/PBD/PBDCollisionOneObject/PBDCollisionOneObjectExample.cpp
+++ b/Examples/PBD/PBDCollisionOneObject/PBDCollisionOneObjectExample.cpp
@@ -79,8 +79,8 @@ main()
auto pbdParams = std::make_shared<PBDModelConfig>();
// FEM constraint
- pbdParams->femParams->m_YoungModulus = 1000.0;
- pbdParams->femParams->m_PoissonRatio = 0.3;
+ pbdParams->m_femParams->m_YoungModulus = 1000.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.3;
pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet,
PbdFEMConstraint::MaterialType::Corotation);
diff --git a/Examples/PBD/PBDCollisionStairs/PBDCollisionStairsExample.cpp b/Examples/PBD/PBDCollisionStairs/PBDCollisionStairsExample.cpp
index 00624070bd192830fb30526b3ea74740196cdb45..efc2653af05b7255f1186d3d80ca688bc510d2b7 100644
--- a/Examples/PBD/PBDCollisionStairs/PBDCollisionStairsExample.cpp
+++ b/Examples/PBD/PBDCollisionStairs/PBDCollisionStairsExample.cpp
@@ -108,8 +108,8 @@ makeDragonPbdObject(const std::string& name)
// Setup the Parameters
auto pbdParams = std::make_shared<PBDModelConfig>();
- pbdParams->femParams->m_YoungModulus = 1000.0;
- pbdParams->femParams->m_PoissonRatio = 0.3;
+ pbdParams->m_femParams->m_YoungModulus = 1000.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.3;
pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet,
PbdFEMConstraint::MaterialType::StVK);
pbdParams->m_uniformMassValue = 1.0;
diff --git a/Examples/PBD/PBDDeformableObject/PBD3DDeformableObject.cpp b/Examples/PBD/PBDDeformableObject/PBD3DDeformableObject.cpp
index b07e73792aabf9ecef7a0c46f7511e5f9b02ceec..51acbdc6056aade31b2b788b6bfef2554640437c 100644
--- a/Examples/PBD/PBDDeformableObject/PBD3DDeformableObject.cpp
+++ b/Examples/PBD/PBDDeformableObject/PBD3DDeformableObject.cpp
@@ -66,15 +66,16 @@ main()
auto pbdParams = std::make_shared<PBDModelConfig>();
// FEM constraint
- pbdParams->femParams->m_YoungModulus = 100.0;
- pbdParams->femParams->m_PoissonRatio = 0.3;
- pbdParams->m_fixedNodeIds = { 51, 127, 178 };
+ pbdParams->m_femParams->m_YoungModulus = 1000.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.3;
+ pbdParams->m_fixedNodeIds = { 50, 126, 177 };
pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet, PbdFEMConstraint::MaterialType::StVK);
// Other parameters
pbdParams->m_uniformMassValue = 1.0;
pbdParams->m_gravity = Vec3d(0, -9.8, 0);
- pbdParams->m_iterations = 45;
+ pbdParams->m_iterations = 15;
+ // pbdParams->m_solverType = PbdConstraint::SolverType::PBD;
// Set the parameters
pbdModel->configure(pbdParams);
diff --git a/Examples/PBD/PBDString/pbdStringExample.cpp b/Examples/PBD/PBDString/pbdStringExample.cpp
index b3b17a9eafb256b3461d54bf6420da5a81d54692..a6b8e2b8a7d1e846c6dd0a94d3daeb8a52c2fd11 100644
--- a/Examples/PBD/PBDString/pbdStringExample.cpp
+++ b/Examples/PBD/PBDString/pbdStringExample.cpp
@@ -91,8 +91,8 @@ main()
// Configure the parameters with bend stiffnesses varying from 0.001 to ~0.1
sims[i].params = std::make_shared<PBDModelConfig>();
- sims[i].params->enableConstraint(PbdConstraint::Type::Distance, 0.001);
- sims[i].params->enableConstraint(PbdConstraint::Type::Bend, static_cast<double>(i) * 0.1 / numStrings + 0.001);
+ sims[i].params->enableConstraint(PbdConstraint::Type::Distance, 1e7);
+ sims[i].params->enableConstraint(PbdConstraint::Type::Bend, (static_cast<double>(i) * 0.1 / numStrings + 0.001) * 1e6);
sims[i].params->m_fixedNodeIds = { 0 };
sims[i].params->m_uniformMassValue = 5.0;
sims[i].params->m_gravity = Vec3d(0, -9.8, 0);
diff --git a/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.cpp
index 1246418adb0619c49ca29a2077ea7fcacb9ab56d..6121b15e54634b7d15e52f26976341c327a21ed0 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.cpp
@@ -21,81 +21,61 @@
#include "imstkPbdAreaConstraint.h"
-namespace imstk
+namespace imstk
{
void
-PbdAreaConstraint::initConstraint(
- const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx1, const size_t& pIdx2, const size_t& pIdx3,
- const double k)
+PbdAreaConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const size_t& pIdx2,
+ const double k)
{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_vertexIds[2] = pIdx3;
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_vertexIds[2] = pIdx2;
- m_stiffness = k;
+ this->m_stiffness = k;
+ this->m_compliance = 1.0 / k;
- const Vec3d& p0 = initVertexPositions[pIdx1];
- const Vec3d& p1 = initVertexPositions[pIdx2];
- const Vec3d& p2 = initVertexPositions[pIdx3];
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+ const Vec3d& p2 = initVertexPositions[pIdx2];
m_restArea = 0.5 * (p1 - p0).cross(p2 - p0).norm();
}
bool
-PbdAreaConstraint::solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses)
+PbdAreaConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
{
const auto i1 = m_vertexIds[0];
const auto i2 = m_vertexIds[1];
const auto i3 = m_vertexIds[2];
- Vec3d& p0 = currVertexPositions[i1];
- Vec3d& p1 = currVertexPositions[i2];
- Vec3d& p2 = currVertexPositions[i3];
+ const Vec3d& p0 = currVertexPositions[i1];
+ const Vec3d& p1 = currVertexPositions[i2];
+ const Vec3d& p2 = currVertexPositions[i3];
- const Real im0 = currInvMasses[i1];
- const Real im1 = currInvMasses[i2];
- const Real im2 = currInvMasses[i3];
+ const Vec3d e0 = p0 - p1;
+ const Vec3d e1 = p1 - p2;
+ const Vec3d e2 = p2 - p0;
- const Vec3d e1 = p0 - p1;
- const Vec3d e2 = p1 - p2;
- const Vec3d e3 = p2 - p0;
+ Vec3d n = e0.cross(e1);
+ c = 0.5 * n.norm();
- Vec3d n = e1.cross(e2);
- const Real A = 0.5 * n.norm();
-
- if (A < m_epsilon)
+ if (c < m_epsilon)
{
return false;
}
- n /= 2 * A;
-
- const Vec3d grad0 = e2.cross(n);
- const Vec3d grad1 = e3.cross(n);
- const Vec3d grad2 = e1.cross(n);
+ n /= 2 * c;
+ c -= m_restArea;
- Real lambda = im0 * grad0.squaredNorm() + im1 * grad1.squaredNorm() + im2 * grad2.squaredNorm();
-
- lambda = (A - m_restArea) / lambda * m_stiffness;
-
- if (im0 > 0)
- {
- p0 += -im0 * lambda * grad0;
- }
-
- if (im1 > 0)
- {
- p1 += -im1 * lambda * grad1;
- }
-
- if (im2 > 0)
- {
- p2 += -im2 * lambda * grad2;
- }
+ dcdx[0] = e1.cross(n);
+ dcdx[1] = e2.cross(n);
+ dcdx[2] = e0.cross(n);
return true;
}
-} // imstk
\ No newline at end of file
+}
diff --git a/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h
index a1c96fd035e62b0c5907c643f4c0a78a61c57dda..0135fe574b3849b46d1e47ee1cd982b4cf252e32 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h
@@ -51,15 +51,11 @@ public:
const size_t& pIdx1, const size_t& pIdx2, const size_t& pIdx3,
const double k = 2.5);
- ///
- /// \brief Solves the area constraint
- ///
- bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses) override;
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
- double m_restArea = 0.; ///> Area at the rest position
- double m_stiffness = 0.; ///> Stiffness of the area constraint
+ double m_restArea = 0.; ///> Area at the rest position
};
} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.cpp
index 5db6715cb5d58b65d9a58e7e60133d71aa178f79..9bb0315a17ef4617bd539746ca0feeda88b6947f 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.cpp
@@ -24,20 +24,22 @@
namespace imstk
{
void
-PbdBendConstraint::initConstraint(
- const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const double k)
+PbdBendConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const size_t& pIdx2,
+ const double k)
{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_vertexIds[2] = pIdx3;
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_vertexIds[2] = pIdx2;
- m_stiffness = k;
+ m_stiffness = k;
+ m_compliance = 1.0 / k;
- const Vec3d& p0 = initVertexPositions[pIdx1];
- const Vec3d& p1 = initVertexPositions[pIdx2];
- const Vec3d& p2 = initVertexPositions[pIdx3];
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+ const Vec3d& p2 = initVertexPositions[pIdx2];
// Instead of using the angle between the segments we can use the distance
// from the center of the triangle
@@ -46,53 +48,35 @@ PbdBendConstraint::initConstraint(
}
bool
-PbdBendConstraint::solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses)
+PbdBendConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
{
- const size_t i1 = m_vertexIds[0];
- const size_t i2 = m_vertexIds[1];
- const size_t i3 = m_vertexIds[2];
+ const size_t i0 = m_vertexIds[0];
+ const size_t i1 = m_vertexIds[1];
+ const size_t i2 = m_vertexIds[2];
- Vec3d& p0 = currVertexPositions[i1];
- Vec3d& p1 = currVertexPositions[i2];
- Vec3d& p2 = currVertexPositions[i3];
-
- const Real im0 = currInvMasses[i1];
- const Real im1 = currInvMasses[i2];
- const Real im2 = currInvMasses[i3];
-
- const Real m0 = (im0 > 0.0) ? 1.0 / im0 : 0.0;
- const Real m1 = (im1 > 0.0) ? 1.0 / im1 : 0.0;
- const Real m2 = (im2 > 0.0) ? 1.0 / im2 : 0.0;
+ const Vec3d& p0 = currVertexPositions[i0];
+ const Vec3d& p1 = currVertexPositions[i1];
+ const Vec3d& p2 = currVertexPositions[i2];
// Move towards triangle center
const Vec3d& center = (p0 + p1 + p2) / 3.0;
const Vec3d& diff = p1 - center;
const double dist = diff.norm();
+
if (dist < m_epsilon)
{
return false;
}
- const Vec3d& dir = diff / dist;
- const double c = (dist - m_restLength) * m_stiffness;
-
- // Now Apply movement weighted by masses
- if (im0 > 0.0)
- {
- p0 += c * dir * 2.0 * m0 / (m0 + m1 + m2);
- }
- if (im1 > 0.0)
- {
- p1 -= c * dir * 4.0 * m1 / (m0 + m1 + m2);
- }
+ c = dist - m_restLength;
- if (im2 > 0.0)
- {
- p2 += c * dir * 2.0 * m2 / (m0 + m1 + m2);
- }
+ dcdx.resize(3);
+ dcdx[0] = (-2.0 / dist) * diff;
+ dcdx[1] = -2.0 * dcdx[0];
+ dcdx[2] = dcdx[0];
return true;
}
-} // imstk
\ No newline at end of file
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h
index 93d70a4b612190d1654d8ec93c0013a9f4a57f98..c2e68dc2f8702512a4f14127834fe361994831f2 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h
@@ -53,7 +53,7 @@ public:
/
p2
\param model
- \param pIdx1 index of p0
+ \param pIdx0 index of p0
\param pIdx2 index of p1
\param pIdx3 index of p2
\param k stiffness
@@ -64,14 +64,12 @@ public:
const size_t& pIdx3, const double k);
///
- /// \brief Solves the bend constraint
+ /// \brief Compute the value and gradient of constraint
///
- bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses) override;
-
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPosition,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
double m_restLength = 0.; ///> Rest length
- double m_stiffness = 0.; ///> Bend stiffness
};
} //imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp
index 529c8103cdb64f2193a53a45cd13be91f296449b..dda274ce9101a8d8b14404fd0ee0bd6062378fa5 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp
@@ -29,4 +29,55 @@ PbdCollisionConstraint::PbdCollisionConstraint(const unsigned int& n1, const uns
m_bodiesFirst.resize(n1);
m_bodiesSecond.resize(n2);
}
-} // imstk
\ No newline at end of file
+
+void
+PbdCollisionConstraint::projectConstraint(const StdVectorOfReal& invMassA,
+ const StdVectorOfReal& invMassB,
+ StdVectorOfVec3d& posA,
+ StdVectorOfVec3d& posB)
+{
+ double c;
+ StdVectorOfVec3d dcdxA;
+ StdVectorOfVec3d dcdxB;
+
+ bool update = this->computeValueAndGradient(posA, posB, c, dcdxA, dcdxB);
+ if (!update)
+ {
+ return;
+ }
+
+ double lambda = 0.0;
+
+ for (size_t i = 0; i < m_bodiesFirst.size(); ++i)
+ {
+ lambda += invMassA[m_bodiesFirst[i]] * dcdxA[i].squaredNorm();
+ }
+
+ for (size_t i = 0; i < m_bodiesSecond.size(); ++i)
+ {
+ lambda += invMassB[m_bodiesSecond[i]] * dcdxB[i].squaredNorm();
+ }
+
+ lambda = c / lambda;
+
+ for (size_t i = 0, vid = 0; i < m_bodiesFirst.size(); ++i)
+ {
+ vid = m_bodiesFirst[i];
+ if (invMassA[vid] > 0.0)
+ {
+ posA[vid] -= invMassA[vid] * lambda * dcdxA[i] * m_configA->m_stiffness;
+ }
+ }
+
+ for (size_t i = 0, vid = 0; i < m_bodiesSecond.size(); ++i)
+ {
+ vid = m_bodiesSecond[i];
+ if (invMassB[vid] > 0.0)
+ {
+ posB[vid] -= invMassB[vid] * lambda * dcdxB[i] * m_configB->m_stiffness;
+ }
+ }
+
+ return;
+}
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h
index 25beedc4823e27b2638d5c21ddebfd9fc65d1e9c..bd5270f7224acfaa8362fa49001b2e5bac916a5a 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h
@@ -62,14 +62,35 @@ public:
virtual ~PbdCollisionConstraint() = default;
///
- /// \brief
+ /// \brief Get vertex indices of first object
+ ///
+ const std::vector<size_t>& getVertexIdsFirst() const { return m_bodiesFirst; }
+ const std::vector<size_t>& getVertexIdsSecond() const { return m_bodiesSecond; }
+
+ ///
+ /// \brief Get config
///
- virtual bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositionsA,
- StdVectorOfVec3d& currVertexPositionsB,
- const StdVectorOfReal& currInvMassesA,
- const StdVectorOfReal& currInvMassesB) = 0;
+ const PbdCollisionConstraintConfig& getConfigFirst() const { return *m_configA; }
+ const PbdCollisionConstraintConfig& getConfigSecond() const { return *m_configB; }
+ ///
+ /// \brief compute value and gradient of constraint function
+ ///
+ /// \param[in] currVertexPositionsA current positions from object A
+ /// \param[in] currVertexPositionsA current positions from object B
+ /// \param[inout] c constraint value
+ /// \param[inout] dcdx constraint gradient
+ ///
+ virtual bool computeValueAndGradient(const StdVectorOfVec3d& posA,
+ const StdVectorOfVec3d& posB,
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const = 0;
+
+ virtual void projectConstraint(const StdVectorOfReal& invMassA,
+ const StdVectorOfReal& invMassB,
+ StdVectorOfVec3d& posA,
+ StdVectorOfVec3d& posB);
protected:
std::vector<size_t> m_bodiesFirst; ///> index of points for the first object
std::vector<size_t> m_bodiesSecond; ///> index of points for the second object
diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.cpp
index 116d54f06490edd333eb7b669e7f9689f2073efc..03fe3fd841019825d024481ad38f58ffb441640d 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.cpp
@@ -44,10 +44,11 @@ PbdConstantDensityConstraint::initConstraint(const StdVectorOfVec3d& initVertexP
m_NeighborSearcher = std::make_shared<NeighborSearch>(m_NeighborSearchMethod, m_maxDist);
}
-bool
-PbdConstantDensityConstraint::solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses)
+void
+PbdConstantDensityConstraint::projectConstraint(const StdVectorOfReal& currInvMasses,
+ const double dt,
+ const PbdConstraint::SolverType& type,
+ StdVectorOfVec3d& currVertexPositions)
{
const size_t numParticles = currVertexPositions.size();
@@ -68,8 +69,6 @@ PbdConstantDensityConstraint::solvePositionConstraint(
[&](const size_t idx) {
updatePositions(currVertexPositions[idx], idx, currVertexPositions);
});
-
- return true;
}
double
diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h
index d5df78118e277daf8da38de48993d8a185827177..81f97b656f2a54f98b8c07ad9640af60e30f50e6 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h
@@ -59,9 +59,17 @@ public:
///
/// \brief Solves the constant density constraint
///
- bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses) override;
+ void projectConstraint(const StdVectorOfReal& currInvMasses,
+ const double dt,
+ const PbdConstraint::SolverType& type,
+ StdVectorOfVec3d& currVertexPositions) override;
+
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
+ {
+ return true;
+ }
private:
///
diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..443c2a17ad140350c8ed738534e314f6a40b1d09
--- /dev/null
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstraint.cpp
@@ -0,0 +1,79 @@
+/*=========================================================================
+
+ 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 "imstkPbdConstraint.h"
+
+namespace imstk
+{
+void
+PbdConstraint::projectConstraint(const StdVectorOfReal& invMasses, const double dt, const SolverType& solverType, StdVectorOfVec3d& pos)
+{
+ double c;
+ StdVectorOfVec3d dcdx;
+
+ bool update = this->computeValueAndGradient(pos, c, dcdx);
+ if (!update)
+ {
+ return;
+ }
+
+ double dcMidc = 0.0;
+ double lambda = 0.0;
+ double alpha;
+
+ for (size_t i = 0; i < m_vertexIds.size(); ++i)
+ {
+ dcMidc += invMasses[m_vertexIds[i]] * dcdx[i].squaredNorm();
+ }
+
+ if (dcMidc < VERY_SMALL_EPSILON)
+ {
+ return;
+ }
+
+ switch (solverType)
+ {
+ case (SolverType::xPBD):
+ alpha = m_compliance / (dt * dt);
+ lambda = -(c + alpha * m_lambda) / (dcMidc + alpha);
+ m_lambda += lambda;
+ break;
+ case (SolverType::PBD):
+ lambda = -c * m_stiffness / dcMidc;
+ break;
+ default:
+ alpha = m_compliance / (dt * dt);
+ lambda = -(c + alpha * m_lambda) / (dcMidc + alpha);
+ m_lambda += lambda;
+ }
+
+ for (size_t i = 0, vid = 0; i < m_vertexIds.size(); ++i)
+ {
+ vid = m_vertexIds[i];
+ if (invMasses[vid] > 0.0)
+ {
+ pos[vid] += invMasses[vid] * lambda * dcdx[i];
+ }
+ }
+
+ return;
+}
+}
diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
index dd556980845d1529c8f9fbdcb96c811bc06f41f0..927fc9eeffc0e5e0172a29a619997a5fd97e0883 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
@@ -47,6 +47,14 @@ public:
None
};
+ ///
+ /// \brief Type of solvers
+ enum class SolverType
+ {
+ xPBD = 0,
+ PBD,
+ GCD
+ };
///
/// \brief Constructor
///
@@ -64,13 +72,15 @@ public:
virtual Type getType() const = 0;
///
- /// \brief compute delta position from the constraint function
- /// \param model \class PbdModel
- /// \return true if succeeded
+ /// \brief Compute value and gradient of the constraint
+ ///
+ /// \param[in] currVertexPositions vector of current positions
+ /// \param[inout] c constraint value
+ /// \param[inout] dcdx constraint gradient
///
- virtual bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses) = 0;
+ virtual bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const = 0;
///
/// \brief Get the vertex indices of the constraint
@@ -87,9 +97,35 @@ public:
///
double getTolerance() const { return m_epsilon; }
+ ///
+ /// \brief Set the stiffness
+ ///
+ void setStiffness(const double stiffness)
+ {
+ m_stiffness = stiffness;
+ m_compliance = 1.0 / stiffness;
+ }
+
+ ///
+ /// \brief Get the stiffness
+ ///
+ double getStiffness() const { return m_stiffness; }
+
+ ///
+ /// \brief Use PBD
+ ///
+ void zeroOutLambda() { m_lambda = 0.0; }
+
+ ///
+ /// \brief Update positions by projecting constraints.
+ ///
+ virtual void projectConstraint(const StdVectorOfReal& currInvMasses, const double dt, const SolverType& type, StdVectorOfVec3d& pos);
protected:
std::vector<size_t> m_vertexIds; ///> index of points for the constraint
- double m_epsilon = 1.0e-16; ///> Tolerance used for the costraints
+ double m_epsilon = 1.0e-16; ///> Tolerance used for the costraints
+ double m_stiffness = 1.0; ///> used in PBD, [0, 1]
+ double m_compliance = 1e-7; ///> used in xPBD, inverse of Young's Modulus
+ mutable double m_lambda = 0.0; ///> Lagrange multiplier
};
using PBDConstraintVector = std::vector<std::shared_ptr<PbdConstraint>>;
diff --git a/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp
index f3e83e96cc6fbbb7fabe827ef26922b60bb3317f..d45e2edd87341aa2e7d24fe589cef5e3411168b4 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp
@@ -20,26 +20,30 @@
=========================================================================*/
#include "imstkPbdDihedralConstraint.h"
+#include <iostream>
namespace imstk
{
void
PbdDihedralConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const size_t& pIdx4,
- const double k)
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const size_t& pIdx2,
+ const size_t& pIdx3,
+ const double k)
{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_vertexIds[2] = pIdx3;
- m_vertexIds[3] = pIdx4;
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_vertexIds[2] = pIdx2;
+ m_vertexIds[3] = pIdx3;
- m_stiffness = k;
+ m_stiffness = k;
+ m_compliance = 1.0 / k;
- const Vec3d& p0 = initVertexPositions[pIdx1];
- const Vec3d& p1 = initVertexPositions[pIdx2];
- const Vec3d& p2 = initVertexPositions[pIdx3];
- const Vec3d& p3 = initVertexPositions[pIdx4];
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+ const Vec3d& p2 = initVertexPositions[pIdx2];
+ const Vec3d& p3 = initVertexPositions[pIdx3];
const Vec3d n1 = (p2 - p0).cross(p3 - p0).normalized();
const Vec3d n2 = (p3 - p1).cross(p2 - p1).normalized();
@@ -48,29 +52,19 @@ PbdDihedralConstraint::initConstraint(const StdVectorOfVec3d& initVertexPosition
}
bool
-PbdDihedralConstraint::solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& invMasses)
+PbdDihedralConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
{
- const auto i1 = m_vertexIds[0];
- const auto i2 = m_vertexIds[1];
- const auto i3 = m_vertexIds[2];
- const auto i4 = m_vertexIds[3];
-
- Vec3d& p0 = currVertexPositions[i1];
- Vec3d& p1 = currVertexPositions[i2];
- Vec3d& p2 = currVertexPositions[i3];
- Vec3d& p3 = currVertexPositions[i4];
-
- const auto im0 = invMasses[i1];
- const auto im1 = invMasses[i2];
- const auto im2 = invMasses[i3];
- const auto im3 = invMasses[i4];
-
- if (im0 == 0.0 && im1 == 0.0)
- {
- return false;
- }
+ const auto i0 = m_vertexIds[0];
+ const auto i1 = m_vertexIds[1];
+ const auto i2 = m_vertexIds[2];
+ const auto i3 = m_vertexIds[3];
+
+ const Vec3d& p0 = currVertexPositions[i0];
+ const Vec3d& p1 = currVertexPositions[i1];
+ const Vec3d& p2 = currVertexPositions[i2];
+ const Vec3d& p3 = currVertexPositions[i3];
const auto e = p3 - p2;
const auto e1 = p3 - p0;
@@ -91,39 +85,14 @@ PbdDihedralConstraint::solvePositionConstraint(
return false;
}
- const Vec3d grad0 = -(l / A1) * n1;
- const Vec3d grad1 = -(l / A2) * n2;
- const Vec3d grad2 = (e.dot(e1) / (A1 * l)) * n1 + (e.dot(e3) / (A2 * l)) * n2;
- const Vec3d grad3 = (e.dot(e2) / (A1 * l)) * n1 + (e.dot(e4) / (A2 * l)) * n2;
-
- auto lambda = im0 * grad0.squaredNorm() +
- im1 * grad1.squaredNorm() +
- im2 * grad2.squaredNorm() +
- im3 * grad3.squaredNorm();
-
- // huge difference if use acos instead of atan2
- lambda = (atan2(n1.cross(n2).dot(e), l * n1.dot(n2)) - m_restAngle) / lambda * m_stiffness;
-
- if (im0 > 0)
- {
- p0 += -im0 * lambda * grad0;
- }
+ dcdx.resize(4);
+ dcdx[0] = -(l / A1) * n1;
+ dcdx[1] = -(l / A2) * n2;
+ dcdx[2] = (e.dot(e1) / (A1 * l)) * n1 + (e.dot(e3) / (A2 * l)) * n2;
+ dcdx[3] = (e.dot(e2) / (A1 * l)) * n1 + (e.dot(e4) / (A2 * l)) * n2;
- if (im1 > 0)
- {
- p1 += -im1 * lambda * grad1;
- }
-
- if (im2 > 0)
- {
- p2 += -im2 * lambda * grad2;
- }
-
- if (im3 > 0)
- {
- p3 += -im3 * lambda * grad3;
- }
+ c = atan2(n1.cross(n2).dot(e), l * n1.dot(n2)) - m_restAngle;
return true;
}
-} // imstk
\ No newline at end of file
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h
index 44c1dc173d99ebab18ce097d93a2a44ce93a7e73..98ad174c203726cf03ff14263504481b79aa5ca4 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h
@@ -61,19 +61,22 @@ public:
*/
void initConstraint(
const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const size_t& pIdx4,
+ const size_t& pIdx0, const size_t& pIdx1,
+ const size_t& pIdx2, const size_t& pIdx3,
const double k);
///
- /// \brief Solves the dihedral angular constraint
+ /// \brief Compute value and gradient of the constraint
///
- bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& invMasses) override;
+ /// \param[in] currVertexPositions vector of current positions
+ /// \param[inout] c constraint value
+ /// \param[inout] dcdx constraint gradient
+ ///
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
double m_restAngle = 0.0; ///> Rest angle
- double m_stiffness = 0.0; ///> Angular stiffness
};
} //imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp
index 7c01fbdbf1a7f38a523547a4ec53aadb2d7dcdd5..f0815882793e062cb43330b643a06f8264a4bf77 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp
@@ -24,56 +24,37 @@
namespace imstk
{
void
-PbdDistanceConstraint::initConstraint(
- const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx1, const size_t& pIdx2, const double k)
+PbdDistanceConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const double k)
{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
m_stiffness = k;
+ m_compliance = 1.0 / k;
+ const Vec3d& p0 = initVertexPositions[pIdx0];
const Vec3d& p1 = initVertexPositions[pIdx1];
- const Vec3d& p2 = initVertexPositions[pIdx2];
- m_restLength = (p1 - p2).norm();
+ m_restLength = (p0 - p1).norm();
}
bool
-PbdDistanceConstraint::solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses)
+PbdDistanceConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
{
- const size_t i1 = m_vertexIds[0];
- const size_t i2 = m_vertexIds[1];
+ const Vec3d& p0 = currVertexPositions[m_vertexIds[0]];
+ const Vec3d& p1 = currVertexPositions[m_vertexIds[1]];
+ dcdx.resize(m_vertexIds.size());
- Vec3d& p0 = currVertexPositions[i1];
- Vec3d& p1 = currVertexPositions[i2];
+ dcdx[0] = p0 - p1;
+ const double len = dcdx[0].norm();
+ dcdx[0] /= len;
+ dcdx[1] = -dcdx[0];
+ c = len - m_restLength;
- const Real im1 = currInvMasses[i1];
- const Real im2 = currInvMasses[i2];
-
- const auto wsum = im1 + im2;
-
- if (wsum == 0.0)
- {
- return false;
- }
-
- Vec3d n = p1 - p0;
- const auto len = n.norm();
- n /= len;
-
- const Vec3d gradC = n * m_stiffness * (len - m_restLength) / wsum;
-
- if (im1 > 0)
- {
- p0 += im1 * gradC;
- }
-
- if (im2 > 0)
- {
- p1 += -im2 * gradC;
- }
return true;
}
-} // imstk
\ No newline at end of file
+}
diff --git a/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h
index 72ccda29de33bfc5d9b969d79cd8cf4aec0f8715..12a7516718723d43461778b97f7ef90b6c3be1b5 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h
@@ -46,19 +46,16 @@ public:
///
/// \brief Initializes the distance constraint
///
- void initConstraint(
- const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx1, const size_t& pIdx2, const double k = 1e-1);
+ void initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const double k = 1e5);
- ///
- /// \brief Solves the Distance constraint
- ///
- bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses) override;
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
double m_restLength = 0.0; ///> Rest length between the nodes
- double m_stiffness = 0.0; ///> Stiffness of the constaint
};
-} // imstk
\ No newline at end of file
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp
index 0aed930f7866a07a40fabe813dc5ce50a356b047..85986e9963e3690f7977c2f6e1854dd387c8b30f 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp
@@ -25,11 +25,12 @@
namespace imstk
{
void
-PbdEdgeEdgeConstraint::initConstraint(
- const size_t& pIdxA1, const size_t& pIdxA2,
- const size_t& pIdxB1, const size_t& pIdxB2,
- std::shared_ptr<PbdCollisionConstraintConfig> configA,
- std::shared_ptr<PbdCollisionConstraintConfig> configB)
+PbdEdgeEdgeConstraint::initConstraint(const size_t& pIdxA1,
+ const size_t& pIdxA2,
+ const size_t& pIdxB1,
+ const size_t& pIdxB2,
+ std::shared_ptr<PbdCollisionConstraintConfig> configA,
+ std::shared_ptr<PbdCollisionConstraintConfig> configB)
{
m_configA = configA;
m_configB = configB;
@@ -40,21 +41,21 @@ PbdEdgeEdgeConstraint::initConstraint(
}
bool
-PbdEdgeEdgeConstraint::solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositionsA,
- StdVectorOfVec3d& currVertexPositionsB,
- const StdVectorOfReal& currInvMassesA,
- const StdVectorOfReal& currInvMassesB)
+PbdEdgeEdgeConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
+ const StdVectorOfVec3d& currVertexPositionsB,
+ double& cc,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const
{
const auto i0 = m_bodiesFirst[0];
const auto i1 = m_bodiesFirst[1];
const auto i2 = m_bodiesSecond[0];
const auto i3 = m_bodiesSecond[1];
- Vec3d& x0 = currVertexPositionsA[i0];
- Vec3d& x1 = currVertexPositionsA[i1];
- Vec3d& x2 = currVertexPositionsB[i2];
- Vec3d& x3 = currVertexPositionsB[i3];
+ const Vec3d& x0 = currVertexPositionsA[i0];
+ const Vec3d& x1 = currVertexPositionsA[i1];
+ const Vec3d& x2 = currVertexPositionsB[i2];
+ const Vec3d& x3 = currVertexPositionsB[i3];
auto a = (x3 - x2).dot(x1 - x0);
auto b = (x1 - x0).dot(x1 - x0);
@@ -72,6 +73,7 @@ PbdEdgeEdgeConstraint::solvePositionConstraint(
t = (c * d - a * f) / det;
if (s < 0 || s > 1.0 || t < 0 || t > 1.0)
{
+ c = 0.0;
return false;
}
}
@@ -91,47 +93,18 @@ PbdEdgeEdgeConstraint::solvePositionConstraint(
if (l > dist)
{
+ c = 0.0;
return false;
}
- Vec3d grad0 = -(1 - t) * n;
- Vec3d grad1 = -(t) * n;
- Vec3d grad2 = (1 - s) * n;
- Vec3d grad3 = (s) * n;
+ dcdxA.resize(2);
+ dcdxB.resize(2);
+ dcdxA[0] = -(1 - t) * n;
+ dcdxA[1] = -(t) * n;
+ dcdxB[0] = (1 - s) * n;
+ dcdxB[1] = (s) * n;
- const auto im0 = currInvMassesA[i0];
- const auto im1 = currInvMassesA[i1];
- const auto im2 = currInvMassesB[i2];
- const auto im3 = currInvMassesB[i3];
-
- auto lambda = im0 * grad0.squaredNorm() +
- im1 * grad1.squaredNorm() +
- im2 * grad2.squaredNorm() +
- im3 * grad3.squaredNorm();
-
- lambda = (l - dist) / lambda;
-
-// LOG(INFO) << "Lambda:" << lambda <<" Normal:" << n[0] <<" " << n[1] <<" "<<n[2];
-
- if (im0 > 0)
- {
- x0 += -im0 * lambda * grad0 * m_configA->m_stiffness;
- }
-
- if (im1 > 0)
- {
- x1 += -im1 * lambda * grad1 * m_configA->m_stiffness;
- }
-
- if (im2 > 0)
- {
- x2 += -im2 * lambda * grad2 * m_configB->m_stiffness;
- }
-
- if (im3 > 0)
- {
- x3 += -im3 * lambda * grad3 * m_configB->m_stiffness;
- }
+ cc = l - dist;
return true;
}
diff --git a/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
index c424f47063976545dce76dc08aaa6f214385d5f2..74b519b35c10956bd23f0e0c7a7e456bf0c61af4 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
@@ -53,12 +53,17 @@ public:
std::shared_ptr<PbdCollisionConstraintConfig> configB);
///
- /// \brief Solve edge-edge collision constraint
+ /// \brief compute value and gradient of constraint function
///
- bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositionsA,
- StdVectorOfVec3d& currVertexPositionsB,
- const StdVectorOfReal& currInvMassesA,
- const StdVectorOfReal& currInvMassesB) override;
+ /// \param[in] currVertexPositionsA current positions from object A
+ /// \param[in] currVertexPositionsA current positions from object B
+ /// \param[inout] c constraint value
+ /// \param[inout] dcdx constraint gradient
+ ///
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
+ const StdVectorOfVec3d& currVertexPositionsB,
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const override;
};
-}
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdFEMConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdFEMConstraint.h
index f71cbc8584097872b9d67c2bd9d3048aa6b420ae..d06dea03362a70a519005fff7082e62c5613da91 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdFEMConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdFEMConstraint.h
@@ -74,6 +74,6 @@ public:
MaterialType m_material; ///> Material type
Mat3d m_invRestMat;
- std::shared_ptr<PbdFEMConstraintConfig> config = nullptr;
+ std::shared_ptr<PbdFEMConstraintConfig> m_config = nullptr;
};
-}
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp
index c76d78852e044cb37d5b34c8b967d32dd7650bc8..4763b2f29c6d5d94db7561aec5eac5eb262a230d 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp
@@ -21,27 +21,29 @@
#include "imstkPbdFETetConstraint.h"
#include "imstkLogger.h"
+#include <iostream>
namespace imstk
{
bool
PbdFEMTetConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const size_t& pIdx4,
+ const size_t& pIdx0, const size_t& pIdx1,
+ const size_t& pIdx2, const size_t& pIdx3,
std::shared_ptr<PbdFEMConstraintConfig> config)
{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_vertexIds[2] = pIdx3;
- m_vertexIds[3] = pIdx4;
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_vertexIds[2] = pIdx2;
+ m_vertexIds[3] = pIdx3;
- const Vec3d& p0 = initVertexPositions[pIdx1];
- const Vec3d& p1 = initVertexPositions[pIdx2];
- const Vec3d& p2 = initVertexPositions[pIdx3];
- const Vec3d& p3 = initVertexPositions[pIdx4];
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+ const Vec3d& p2 = initVertexPositions[pIdx2];
+ const Vec3d& p3 = initVertexPositions[pIdx3];
m_elementVolume = (1.0 / 6.0) * (p3 - p0).dot((p1 - p0).cross(p2 - p0));
- this->config = config;
+ m_config = config;
+ m_compliance = 1.0 / (config->m_lambda + 2 * config->m_mu);
Mat3d m;
m.col(0) = p0 - p3;
@@ -59,21 +61,19 @@ PbdFEMTetConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
}
bool
-PbdFEMTetConstraint::solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses)
+PbdFEMTetConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& cval,
+ StdVectorOfVec3d& dcdx) const
{
- const auto i1 = m_vertexIds[0];
- const auto i2 = m_vertexIds[1];
- const auto i3 = m_vertexIds[2];
- const auto i4 = m_vertexIds[3];
+ const auto i0 = m_vertexIds[0];
+ const auto i1 = m_vertexIds[1];
+ const auto i2 = m_vertexIds[2];
+ const auto i3 = m_vertexIds[3];
- Vec3d& p0 = currVertexPositions[i1];
- Vec3d& p1 = currVertexPositions[i2];
- Vec3d& p2 = currVertexPositions[i3];
- Vec3d& p3 = currVertexPositions[i4];
-
- //double currentVolume = (1.0 / 6.0) * (p3 - p0).dot((p1 - p0).cross(p2 - p0));
+ const Vec3d& p0 = currVertexPositions[i0];
+ const Vec3d& p1 = currVertexPositions[i1];
+ const Vec3d& p2 = currVertexPositions[i2];
+ const Vec3d& p3 = currVertexPositions[i3];
Mat3d m;
m.col(0) = p0 - p3;
@@ -87,8 +87,8 @@ PbdFEMTetConstraint::solvePositionConstraint(
// energy constraint
double C = 0;
- const auto mu = config->m_mu;
- const auto lambda = config->m_lambda;
+ const auto mu = m_config->m_mu;
+ const auto lambda = m_config->m_lambda;
switch (m_material)
{
@@ -174,46 +174,14 @@ PbdFEMTetConstraint::solvePositionConstraint(
}
}
- const double im1 = currInvMasses[i1];
- const double im2 = currInvMasses[i2];
- const double im3 = currInvMasses[i3];
- const double im4 = currInvMasses[i4];
-
Mat3d gradC = m_elementVolume * P * m_invRestMat.transpose();
-
- const double sum = im1 * gradC.col(0).squaredNorm()
- + im2 * gradC.col(1).squaredNorm()
- + im3 * gradC.col(2).squaredNorm()
- + im4 * (gradC.col(0) + gradC.col(1) + gradC.col(2)).squaredNorm();
-
- if (sum < m_epsilon)
- {
- return false;
- }
-
- C = C * m_elementVolume;
-
- const double s = C / sum;
-
- if (im1 > 0)
- {
- p0 += -s* im1* gradC.col(0);
- }
-
- if (im2 > 0)
- {
- p1 += -s* im2* gradC.col(1);
- }
-
- if (im3 > 0)
- {
- p2 += -s* im3* gradC.col(2);
- }
-
- if (im4 > 0)
- {
- p3 += s * im4 * (gradC.col(0) + gradC.col(1) + gradC.col(2));
- }
+ cval = C;
+ cval *= m_elementVolume;
+ dcdx.resize(m_vertexIds.size());
+ dcdx[0] = gradC.col(0);
+ dcdx[1] = gradC.col(1);
+ dcdx[2] = gradC.col(2);
+ dcdx[3] = -dcdx[0] - dcdx[1] - dcdx[2];
return true;
}
diff --git a/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h
index fa52ece9d3b6de25d4f7c308a244013a38ca4cd1..b2dc8e3d2e0111e9884f275844c7b15f216e0084 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h
@@ -54,10 +54,10 @@ public:
std::shared_ptr<PbdFEMConstraintConfig> config);
///
- /// \brief Solve the tetrahedral FEM constraint
+ /// \brief Compute the value and gradient of constraint
///
- bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses) override;
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPosition,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
};
} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp
index 3345de7bfc5ce8b8128babe323f3df1e028d90f6..75b13a8d9e49a1a6ab776c8a3cd7b9e55464758a 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp
@@ -26,8 +26,10 @@
namespace imstk
{
void
-PbdPointTriangleConstraint::initConstraint(const size_t& pIdxA1,
- const size_t& pIdxB1, const size_t& pIdxB2, const size_t& pIdxB3,
+PbdPointTriangleConstraint::initConstraint(const size_t& pIdxA1,
+ const size_t& pIdxB1,
+ const size_t& pIdxB2,
+ const size_t& pIdxB3,
std::shared_ptr<PbdCollisionConstraintConfig> configA,
std::shared_ptr<PbdCollisionConstraintConfig> configB)
{
@@ -40,21 +42,21 @@ PbdPointTriangleConstraint::initConstraint(const size_t& pIdxA1,
}
bool
-PbdPointTriangleConstraint::solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositionsA,
- StdVectorOfVec3d& currVertexPositionsB,
- const StdVectorOfReal& currInvMassesA,
- const StdVectorOfReal& currInvMassesB)
+PbdPointTriangleConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
+ const StdVectorOfVec3d& currVertexPositionsB,
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const
{
const size_t i0 = m_bodiesFirst[0];
const size_t i1 = m_bodiesSecond[0];
const size_t i2 = m_bodiesSecond[1];
const size_t i3 = m_bodiesSecond[2];
- Vec3d& x0 = currVertexPositionsA[i0];
- Vec3d& x1 = currVertexPositionsB[i1];
- Vec3d& x2 = currVertexPositionsB[i2];
- Vec3d& x3 = currVertexPositionsB[i3];
+ const Vec3d& x0 = currVertexPositionsA[i0];
+ const Vec3d& x1 = currVertexPositionsB[i1];
+ const Vec3d& x2 = currVertexPositionsB[i2];
+ const Vec3d& x3 = currVertexPositionsB[i3];
const Vec3d x12 = x2 - x1;
const Vec3d x13 = x3 - x1;
@@ -67,6 +69,7 @@ PbdPointTriangleConstraint::solvePositionConstraint(
if (alpha < 0 || beta < 0 || alpha + beta > 1)
{
//LOG(WARNING) << "Projection point not inside the triangle";
+ c = 0.0;
return false;
}
@@ -78,49 +81,19 @@ PbdPointTriangleConstraint::solvePositionConstraint(
if (l > dist)
{
+ c = 0.0;
return false;
}
double gamma = 1.0 - alpha - beta;
- Vec3d grad0 = n;
- Vec3d grad1 = -alpha * n;
- Vec3d grad2 = -beta * n;
- Vec3d grad3 = -gamma * n;
-
- const Real im0 = currInvMassesA[i0];
- const Real im1 = currInvMassesB[i1];
- const Real im2 = currInvMassesB[i2];
- const Real im3 = currInvMassesB[i3];
-
- double lambda = im0 * grad0.squaredNorm() +
- im1 * grad1.squaredNorm() +
- im2 * grad2.squaredNorm() +
- im3 * grad3.squaredNorm();
-
- lambda = (l - dist) / lambda;
-
- //LOG(INFO) << "Lambda:" << lambda <<" Normal:" << n[0] <<" " << n[1] <<" "<<n[2];
-
- if (im0 > 0)
- {
- x0 += -im0 * lambda * grad0 * m_configA->m_stiffness;
- }
-
- if (im1 > 0)
- {
- x1 += -im1 * lambda * grad1 * m_configB->m_stiffness;
- }
-
- if (im2 > 0)
- {
- x2 += -im2 * lambda * grad2 * m_configB->m_stiffness;
- }
-
- if (im3 > 0)
- {
- x3 += -im3 * lambda * grad3 * m_configB->m_stiffness;
- }
-
+ dcdxA.resize(1);
+ dcdxB.resize(3);
+ dcdxA[0] = n;
+ dcdxB[0] = -alpha * n;
+ dcdxB[1] = -beta * n;
+ dcdxB[2] = -gamma * n;
+
+ c = l - dist;
return true;
}
} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
index c4e7af7fb920705a70a84e1d2f17639459bbff03..dc6186775c3ce99bc1fa00956c3ff39a6a5c7631 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
@@ -56,12 +56,17 @@ public:
std::shared_ptr<PbdCollisionConstraintConfig> configB);
///
- /// \brief
+ /// \brief compute value and gradient of constraint function
///
- bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositionsA,
- StdVectorOfVec3d& currVertexPositionsB,
- const StdVectorOfReal& currInvMassesA,
- const StdVectorOfReal& currInvMassesB) override;
+ /// \param[in] currVertexPositionsA current positions from object A
+ /// \param[in] currVertexPositionsA current positions from object B
+ /// \param[inout] c constraint value
+ /// \param[inout] dcdx constraint gradient
+ ///
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
+ const StdVectorOfVec3d& currVertexPositionsB,
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const override;
};
-}
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
index 83be1d9feb77a37db106c7763aee10c2a791520b..ef63215ebcfa6e58be138a90c170eb9f9012941f 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
@@ -25,81 +25,49 @@ namespace imstk
{
void
PbdVolumeConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx1, const size_t& pIdx2,
- const size_t& pIdx3, const size_t& pIdx4,
- double k)
+ const size_t& pIdx0, const size_t& pIdx1,
+ const size_t& pIdx2, const size_t& pIdx3,
+ const double k)
{
- m_vertexIds[0] = pIdx1;
- m_vertexIds[1] = pIdx2;
- m_vertexIds[2] = pIdx3;
- m_vertexIds[3] = pIdx4;
+ m_vertexIds[0] = pIdx0;
+ m_vertexIds[1] = pIdx1;
+ m_vertexIds[2] = pIdx2;
+ m_vertexIds[3] = pIdx3;
- m_stiffness = k;
+ m_stiffness = k;
+ m_compliance = 1.0 / k;
- const Vec3d& p0 = initVertexPositions[pIdx1];
- const Vec3d& p1 = initVertexPositions[pIdx2];
- const Vec3d& p2 = initVertexPositions[pIdx3];
- const Vec3d& p3 = initVertexPositions[pIdx4];
+ const Vec3d& p0 = initVertexPositions[pIdx0];
+ const Vec3d& p1 = initVertexPositions[pIdx1];
+ const Vec3d& p2 = initVertexPositions[pIdx2];
+ const Vec3d& p3 = initVertexPositions[pIdx3];
m_restVolume = (1.0 / 6.0) * ((p1 - p0).cross(p2 - p0)).dot(p3 - p0);
}
bool
-PbdVolumeConstraint::solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses)
+PbdVolumeConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
{
- const auto i1 = m_vertexIds[0];
- const auto i2 = m_vertexIds[1];
- const auto i3 = m_vertexIds[2];
- const auto i4 = m_vertexIds[3];
+ const auto i0 = m_vertexIds[0];
+ const auto i1 = m_vertexIds[1];
+ const auto i2 = m_vertexIds[2];
+ const auto i3 = m_vertexIds[3];
- Vec3d& x1 = currVertexPositions[i1];
- Vec3d& x2 = currVertexPositions[i2];
- Vec3d& x3 = currVertexPositions[i3];
- Vec3d& x4 = currVertexPositions[i4];
-
- const auto im1 = currInvMasses[i1];
- const auto im2 = currInvMasses[i2];
- const auto im3 = currInvMasses[i3];
- const auto im4 = currInvMasses[i4];
+ const Vec3d& x0 = currVertexPositions[i0];
+ const Vec3d& x1 = currVertexPositions[i1];
+ const Vec3d& x2 = currVertexPositions[i2];
+ const Vec3d& x3 = currVertexPositions[i3];
const double onesixth = 1.0 / 6.0;
- const Vec3d grad1 = onesixth * (x2 - x3).cross(x4 - x2);
- const Vec3d grad2 = onesixth * (x3 - x1).cross(x4 - x1);
- const Vec3d grad3 = onesixth * (x4 - x1).cross(x2 - x1);
- const Vec3d grad4 = onesixth * (x2 - x1).cross(x3 - x1);
-
- const auto V = grad4.dot(x4 - x1);
-
- double lambda = im1 * grad1.squaredNorm() +
- im2 * grad2.squaredNorm() +
- im3 * grad3.squaredNorm() +
- im4 * grad4.squaredNorm();
-
- lambda = (V - m_restVolume) / lambda * m_stiffness;
-
- if (im1 > 0)
- {
- x1 += -im1 * lambda * grad1;
- }
-
- if (im2 > 0)
- {
- x2 += -im2 * lambda * grad2;
- }
-
- if (im3 > 0)
- {
- x3 += -im3 * lambda * grad3;
- }
-
- if (im4 > 0)
- {
- x4 += -im4 * lambda * grad4;
- }
+ dcdx[0] = onesixth * (x1 - x2).cross(x3 - x1);
+ dcdx[1] = onesixth * (x2 - x0).cross(x3 - x0);
+ dcdx[2] = onesixth * (x3 - x0).cross(x1 - x0);
+ dcdx[3] = onesixth * (x1 - x0).cross(x2 - x0);
+ c = dcdx[3].dot(x3 - x0);
return true;
}
-} // imstk
\ No newline at end of file
+} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h
index 30b2c4ea47b974da3d92cc2ce57f4cedd1aca516..7f92dc4eba9a35b8e006476e9628dd1950300daa 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h
@@ -49,17 +49,16 @@ public:
void initConstraint(const StdVectorOfVec3d& initVertexPositions,
const size_t& pIdx1, const size_t& pIdx2,
const size_t& pIdx3, const size_t& pIdx4,
- double k = 2.0);
+ const double k = 2.0);
///
- /// \brief Solves the volume constraint
+ /// \brief Compute the value and gradient of constraint
///
- bool solvePositionConstraint(
- StdVectorOfVec3d& currVertexPositions,
- const StdVectorOfReal& currInvMasses) override;
+ bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPosition,
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
protected:
double m_restVolume = 0.0; ///> Rest volume
- double m_stiffness = 1.0;
};
-}
+} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp b/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
index 194e1299ca8c77971ecbb16c8483167d9e4d6452..a5a583654b712f1d1df16e669756a55adaa4e6d8 100644
--- a/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
@@ -44,7 +44,7 @@ PbdModel::PbdModel() : DynamicalModel(DynamicalModelType::PositionBasedDynamics)
m_partitionedConstraints(std::make_shared<std::vector<PBDConstraintVector>>()),
m_mass(std::make_shared<StdVectorOfReal>()),
m_invMass(std::make_shared<StdVectorOfReal>()),
- m_Parameters(std::make_shared<PBDModelConfig>())
+ m_parameters(std::make_shared<PBDModelConfig>())
{
m_validGeometryTypes = {
Geometry::Type::PointSet,
@@ -66,7 +66,7 @@ PBDModelConfig::enableConstraint(PbdConstraint::Type type, double stiffness)
{
LOG_IF(FATAL, (type == PbdConstraint::Type::FEMTet || type == PbdConstraint::Type::FEMHex))
<< "FEM constraint should be enabled by the enableFEMConstraint function";
- m_RegularConstraints.push_back({ type, stiffness });
+ m_regularConstraints.push_back({ type, stiffness });
}
void
@@ -82,7 +82,7 @@ PbdModel::configure(std::shared_ptr<PBDModelConfig> params)
{
LOG_IF(FATAL, (!this->getModelGeometry())) << "PbdModel::configure - Set PBD Model geometry before configuration!";
- m_Parameters = params;
+ m_parameters = params;
this->setNumDegreeOfFreedom(std::dynamic_pointer_cast<PointSet>(m_geometry)->getNumVertices() * 3);
}
@@ -104,9 +104,9 @@ PbdModel::initialize()
m_mass->resize(numParticles, 0);
m_invMass->resize(numParticles, 0);
- setUniformMass(m_Parameters->m_uniformMassValue);
+ setUniformMass(m_parameters->m_uniformMassValue);
- for (auto i : m_Parameters->m_fixedNodeIds)
+ for (auto i : m_parameters->m_fixedNodeIds)
{
setFixedPoint(i);
}
@@ -117,15 +117,17 @@ PbdModel::initialize()
if (m_pbdSolver == nullptr)
{
m_pbdSolver = std::make_shared<PbdSolver>();
- m_pbdSolver->setIterations(m_Parameters->m_iterations);
+ m_pbdSolver->setIterations(m_parameters->m_iterations);
+ m_pbdSolver->setSolverType(m_parameters->m_solverType);
}
m_pbdSolver->setPositions(getCurrentState()->getPositions());
m_pbdSolver->setInvMasses(getInvMasses());
m_pbdSolver->setConstraints(getConstraints());
m_pbdSolver->setPartitionedConstraints(getPartitionedConstraints());
+ m_pbdSolver->setTimeStep(m_parameters->m_dt);
// Initialize FEM constraints
- for (auto& constraint: m_Parameters->m_FEMConstraints)
+ for (auto& constraint: m_parameters->m_FEMConstraints)
{
computeElasticConstants();
if (!initializeFEMConstraints(constraint.second))
@@ -135,8 +137,17 @@ PbdModel::initialize()
}
// Initialize other constraints
- for (auto& constraint: m_Parameters->m_RegularConstraints)
+ for (auto& constraint: m_parameters->m_regularConstraints)
{
+ if (m_parameters->m_solverType == PbdConstraint::SolverType::PBD && constraint.second > 1.0)
+ {
+ LOG(WARNING) << "for PBD, k should be between [0, 1]";
+ }
+ else if (m_parameters->m_solverType == PbdConstraint::SolverType::xPBD && constraint.second <= 1.0)
+ {
+ LOG(WARNING) << "for xPBD, k is Young's Modulu, and should be much larger than 1";
+ }
+
if (!bOK)
{
return false;
@@ -173,7 +184,11 @@ PbdModel::initialize()
}
// Partition constraints for parallel computation
- partitionConstraints();
+ if (!m_partitioned)
+ {
+ this->partitionConstraints();
+ m_partitioned = true;
+ }
this->setTimeStepSizeType(m_timeStepSizeType);
@@ -194,20 +209,20 @@ PbdModel::initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskN
void
PbdModel::computeElasticConstants()
{
- if (std::abs(m_Parameters->femParams->m_mu) < MIN_REAL
- && std::abs(m_Parameters->femParams->m_lambda) < MIN_REAL)
+ if (std::abs(m_parameters->m_femParams->m_mu) < MIN_REAL
+ && std::abs(m_parameters->m_femParams->m_lambda) < MIN_REAL)
{
- const auto E = m_Parameters->femParams->m_YoungModulus;
- const auto nu = m_Parameters->femParams->m_PoissonRatio;
- m_Parameters->femParams->m_mu = E / Real(2.0) / (Real(1.0) + nu);
- m_Parameters->femParams->m_lambda = E * nu / ((Real(1.0) + nu) * (Real(1.0) - Real(2.0) * nu));
+ const auto E = m_parameters->m_femParams->m_YoungModulus;
+ const auto nu = m_parameters->m_femParams->m_PoissonRatio;
+ m_parameters->m_femParams->m_mu = E / Real(2.0) / (Real(1.0) + nu);
+ m_parameters->m_femParams->m_lambda = E * nu / ((Real(1.0) + nu) * (Real(1.0) - Real(2.0) * nu));
}
else
{
- const auto mu = m_Parameters->femParams->m_mu;
- const auto lambda = m_Parameters->femParams->m_lambda;
- m_Parameters->femParams->m_YoungModulus = mu * (Real(3.0) * lambda + Real(2.0) * mu) / (lambda + mu);
- m_Parameters->femParams->m_PoissonRatio = lambda / Real(2.0) / (lambda + mu);
+ const auto mu = m_parameters->m_femParams->m_mu;
+ const auto lambda = m_parameters->m_femParams->m_lambda;
+ m_parameters->m_femParams->m_YoungModulus = mu * (Real(3.0) * lambda + Real(2.0) * mu) / (lambda + mu);
+ m_parameters->m_femParams->m_PoissonRatio = lambda / Real(2.0) / (lambda + mu);
}
}
@@ -229,7 +244,7 @@ PbdModel::initializeFEMConstraints(PbdFEMConstraint::MaterialType type)
auto& 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->femParams);
+ tet[0], tet[1], tet[2], tet[3], m_parameters->m_femParams);
lock.lock();
m_constraints->push_back(std::move(c));
lock.unlock();
@@ -496,7 +511,10 @@ void
PbdModel::partitionConstraints(const bool print)
{
// Form the map { vertex : list_of_constraints_involve_vertex }
- PBDConstraintVector& allConstraints = *m_constraints;
+ 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)
{
@@ -524,7 +542,7 @@ PbdModel::partitionConstraints(const bool print)
vertexConstraints.clear();
// do graph coloring for the constraint graph
- const auto coloring = constraintGraph.doColoring();
+ const auto coloring = constraintGraph.doColoring(Graph::ColoringMethod::WelshPowell);
const auto& partitionIndices = coloring.first;
const auto numPartitions = coloring.second;
assert(partitionIndices.size() == allConstraints.size());
@@ -600,7 +618,7 @@ PbdModel::setTimeStepSizeType(const TimeSteppingType type)
m_timeStepSizeType = type;
if (type == TimeSteppingType::Fixed)
{
- m_Parameters->m_dt = m_Parameters->m_defaultDt;
+ m_parameters->m_dt = m_parameters->m_defaultDt;
}
}
@@ -655,9 +673,9 @@ PbdModel::integratePosition()
{
if (std::abs(invMasses[i]) > MIN_REAL)
{
- vel[i] += (accn[i] + m_Parameters->m_gravity) * m_Parameters->m_dt;
+ vel[i] += (accn[i] + m_parameters->m_gravity) * m_parameters->m_dt;
prevPos[i] = pos[i];
- pos[i] += (1.0 - m_Parameters->m_viscousDampingCoeff) * vel[i] * m_Parameters->m_dt;
+ pos[i] += (1.0 - m_parameters->m_viscousDampingCoeff) * vel[i] * m_parameters->m_dt;
}
});
}
@@ -673,9 +691,9 @@ PbdModel::updateVelocity()
ParallelUtils::parallelFor(m_mesh->getNumVertices(),
[&](const size_t i)
{
- if (std::abs(invMasses[i]) > MIN_REAL && m_Parameters->m_dt > 0.0)
+ if (std::abs(invMasses[i]) > MIN_REAL && m_parameters->m_dt > 0.0)
{
- vel[i] = (pos[i] - prevPos[i]) / m_Parameters->m_dt;
+ vel[i] = (pos[i] - prevPos[i]) / m_parameters->m_dt;
}
});
}
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdModel.h b/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
index 4b837b16c0d3b57113ec485ccba37c711b1d9206..f141c33ff51736e1d620e1963615fb63e45a24bc 100644
--- a/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
@@ -25,6 +25,7 @@
#include "imstkPbdCollisionConstraint.h"
#include "imstkPbdFEMConstraint.h"
#include "imstkPbdState.h"
+#include "imstkLogger.h"
namespace imstk
{
@@ -55,7 +56,7 @@ struct PBDModelConfig
std::vector<std::size_t> m_fixedNodeIds; ///> Nodal IDs of the nodes that are fixed
Vec3r m_gravity = Vec3r(0, -9.81, 0); ///> Gravity
- std::shared_ptr<PbdFEMConstraintConfig> femParams =
+ std::shared_ptr<PbdFEMConstraintConfig> m_femParams =
std::make_shared<PbdFEMConstraintConfig>(PbdFEMConstraintConfig
{
0.0, // Lame constant, if constraint type is FEM
@@ -64,7 +65,7 @@ struct PBDModelConfig
0.2 // FEM parameter, if constraint type is FEM
}); ///> Info shared between the fem constraints
- std::vector<std::pair<PbdConstraint::Type, double>> m_RegularConstraints; ///> Constraints except FEM
+ std::vector<std::pair<PbdConstraint::Type, double>> m_regularConstraints; ///> Constraints except FEM
std::vector<std::pair<PbdConstraint::Type, PbdFEMConstraint::MaterialType>> m_FEMConstraints; ///> Constraints except FEM
///
@@ -77,6 +78,23 @@ struct PBDModelConfig
/// \brief Enable a FEM constraint with mu, lambda
///
void enableFEMConstraint(PbdConstraint::Type type, PbdFEMConstraint::MaterialType material);
+
+ ///
+ /// \brief Set the PBD solver type
+ ///
+ void setSolverType(const PbdConstraint::SolverType& type)
+ {
+ if (type == PbdConstraint::SolverType::GCD)
+ {
+ LOG(WARNING) << "GCD is NOT implemented yet, use xPBD instead";
+ m_solverType = PbdConstraint::SolverType::xPBD;
+ return;
+ }
+
+ m_solverType = type;
+ }
+
+ PbdConstraint::SolverType m_solverType = PbdConstraint::SolverType::xPBD;
};
///
@@ -105,7 +123,7 @@ public:
///
/// \brief Get the simulation parameters
///
- const std::shared_ptr<PBDModelConfig>& getParameters() const { assert(m_Parameters); return m_Parameters; }
+ const std::shared_ptr<PBDModelConfig>& getParameters() const { assert(m_parameters); return m_parameters; }
///
/// \brief Compute elastic constants: Young Modulus, Poisson Ratio, first and second Lame
@@ -167,8 +185,8 @@ public:
///
/// \brief Set the time step size
///
- virtual void setTimeStep(const Real timeStep) override { m_Parameters->m_dt = timeStep; }
- void setDefaultTimeStep(const Real timeStep) { m_Parameters->m_defaultDt = static_cast<Real>(timeStep); }
+ virtual void setTimeStep(const Real timeStep) override { m_parameters->m_dt = timeStep; }
+ void setDefaultTimeStep(const Real timeStep) { m_parameters->m_defaultDt = static_cast<Real>(timeStep); }
///
/// \brief Set the time step size to fixed size
@@ -178,8 +196,8 @@ public:
///
/// \brief Returns the time step size
///
- virtual double getTimeStep() const override { return m_Parameters->m_dt; }
- double getDefaultTimeStep() const { return m_Parameters->m_defaultDt; }
+ virtual double getTimeStep() const override { return m_parameters->m_dt; }
+ double getDefaultTimeStep() const { return m_parameters->m_defaultDt; }
///
/// \brief
@@ -263,6 +281,7 @@ protected:
void initGraphEdges(std::shared_ptr<TaskNode> source, std::shared_ptr<TaskNode> sink) override;
protected:
+ bool m_partitioned = false; /// \todo this is used in initialize() as a temporary fix to problems on linux
size_t m_partitionThreshold = 16; ///> Threshold for constraint partitioning
std::shared_ptr<PbdSolver> m_pbdSolver = nullptr; ///> PBD solver
@@ -272,7 +291,7 @@ protected:
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
+ std::shared_ptr<PBDModelConfig> m_parameters = nullptr; ///> Model parameters, must be set before simulation
protected:
// Computational Nodes
diff --git a/Source/Rendering/VTKRenderer/imstkVTKRenderer.h b/Source/Rendering/VTKRenderer/imstkVTKRenderer.h
index 99868221a18399980496c509932ae8901e25d461..80805ca35b229869c5d7783f7dda3ffee12ae54d 100644
--- a/Source/Rendering/VTKRenderer/imstkVTKRenderer.h
+++ b/Source/Rendering/VTKRenderer/imstkVTKRenderer.h
@@ -167,11 +167,11 @@ protected:
#ifdef iMSTK_ENABLE_VR
std::vector<vtkOpenVRCameraPose> m_camPos;
#endif
-
vtkSmartPointer<vtkChartXY> m_timeTableChart = nullptr;
vtkSmartPointer<vtkContextActor> m_timeTableChartActor = nullptr;
vtkSmartPointer<vtkTable> m_timeTable = nullptr;
vtkPlotBar* m_timeTablePlot = nullptr;
int m_timeTableIter = 0;
+
};
}
diff --git a/Source/Scene/imstkScene.h b/Source/Scene/imstkScene.h
index 749aca0adbd5b89e49f2ffcdf6804ced1ad4c35a..9bb077ca445aa4587bac8dfca99678605839f1ea 100644
--- a/Source/Scene/imstkScene.h
+++ b/Source/Scene/imstkScene.h
@@ -281,8 +281,8 @@ protected:
NamedMap<VisualModel> m_DebugRenderModelMap;
NamedMap<Light> m_lightsMap;
std::shared_ptr<IBLProbe> m_globalIBLProbe = nullptr;
- std::shared_ptr<Camera> m_camera = std::make_shared<Camera>();
- std::shared_ptr<CollisionGraph> m_collisionGraph = std::make_shared<CollisionGraph>();
+ std::shared_ptr<Camera> m_camera = nullptr;
+ std::shared_ptr<CollisionGraph> m_collisionGraph = nullptr;
std::vector<std::shared_ptr<SceneObjectControllerBase>> m_objectControllers; ///> List of object controllers
std::vector<std::shared_ptr<CameraController>> m_cameraControllers; ///> List of camera controllers
std::unordered_map<std::string, std::thread> m_threadMap; ///>
diff --git a/Source/SimulationManager/VTKRenderer/imstkVTKViewer.h b/Source/SimulationManager/VTKRenderer/imstkVTKViewer.h
index 7873b600521f8e7d02523688f402b8a1017001df..0bb255069f5d61a3dbad29c0d3491f71270c5a81 100644
--- a/Source/SimulationManager/VTKRenderer/imstkVTKViewer.h
+++ b/Source/SimulationManager/VTKRenderer/imstkVTKViewer.h
@@ -115,7 +115,7 @@ protected:
std::shared_ptr<VTKInteractorStyle> m_vtkInteractorStyle;
bool m_enableVR;
std::string m_windowName = "imstk";
- vtkSmartPointer<vtkCallbackCommand> timerCallbackCommand = nullptr;
+ vtkSmartPointer<vtkCallbackCommand> timerCallbackCommand;
#ifdef iMSTK_ENABLE_VR
vtkSmartPointer<OpenVRCommand> m_openVRCommand;
diff --git a/Source/Solvers/imstkPbdSolver.cpp b/Source/Solvers/imstkPbdSolver.cpp
index 25accfa1988a429ad1ad5d9188014056f7535dd6..a03ff5351351d8448fa6371394dc1600eaaffe29 100644
--- a/Source/Solvers/imstkPbdSolver.cpp
+++ b/Source/Solvers/imstkPbdSolver.cpp
@@ -42,23 +42,36 @@ PbdSolver::solve()
const PBDConstraintVector& constraints = *m_constraints;
const std::vector<PBDConstraintVector>& partitionedConstraints = *m_partitionedConstraints;
+ // zero out the Lagrange multiplier
+ for (size_t j = 0; j < constraints.size(); ++j)
+ {
+ constraints[j]->zeroOutLambda();
+ }
+
+ for (size_t j = 0; j < partitionedConstraints.size(); j++)
+ {
+ const PBDConstraintVector& constraintPartition = partitionedConstraints[j];
+ ParallelUtils::parallelFor(constraintPartition.size(),
+ [&](const size_t idx) { constraintPartition[idx]->zeroOutLambda(); }
+ );
+ }
+
unsigned int i = 0;
while (i++ < m_iterations)
{
- for (size_t j = 0; j < constraints.size(); j++)
+ for (size_t j = 0; j < constraints.size(); ++j)
{
- constraints[j]->solvePositionConstraint(currPositions, invMasses);
+ constraints[j]->projectConstraint(invMasses, m_dt, m_solverType, currPositions);
}
for (size_t j = 0; j < partitionedConstraints.size(); j++)
{
- const PBDConstraintVector& constraintPartition = partitionedConstraints[j];
- const size_t numConstraintsInPartition = constraintPartition.size();
+ const PBDConstraintVector& constraintPartition = partitionedConstraints[j];
ParallelUtils::parallelFor(constraintPartition.size(),
[&](const size_t idx)
{
- constraintPartition[idx]->solvePositionConstraint(currPositions, invMasses);
+ constraintPartition[idx]->projectConstraint(invMasses, m_dt, m_solverType, currPositions);
});
// Sequential
/*for (size_t k = 0; k < constraintPartition.size(); k++)
@@ -104,7 +117,7 @@ PbdCollisionSolver::solve()
const PBDCollisionConstraintVector& constraints = *constraintList;
for (size_t j = 0; j < constraints.size(); j++)
{
- constraints[j]->solvePositionConstraint(posA, posB, invMassA, invMassB);
+ constraints[j]->projectConstraint(invMassA, invMassB, posA, posB);
}
colDataIter++;
}
diff --git a/Source/Solvers/imstkPbdSolver.h b/Source/Solvers/imstkPbdSolver.h
index 0622a90ec0911e88eed660e2da4c17ddce234826..f7012d6b5766bd9e6315db03f85812299793909b 100644
--- a/Source/Solvers/imstkPbdSolver.h
+++ b/Source/Solvers/imstkPbdSolver.h
@@ -24,6 +24,7 @@
#include "imstkPbdCollisionConstraint.h"
#include "imstkPbdConstraint.h"
#include "imstkSolverBase.h"
+#include "imstkLogger.h"
namespace imstk
{
@@ -87,11 +88,31 @@ public:
///
void setInvMasses(std::shared_ptr<StdVectorOfReal> invMasses) { this->m_invMasses = invMasses; }
+ ///
+ /// \brief Set time step
+ ///
+ void setTimeStep(const double dt) { m_dt = dt; }
+
///
/// \brief Get Iterations. Returns current nonlinear iterations.
///
size_t getIterations() const { return this->m_iterations; }
+ ///
+ /// \brief Set the PBD solver type
+ ///
+ void setSolverType(const PbdConstraint::SolverType& type)
+ {
+ if (type == PbdConstraint::SolverType::GCD)
+ {
+ LOG(WARNING) << "GCD is NOT implemented yet, use xPBD instead";
+ m_solverType = PbdConstraint::SolverType::xPBD;
+ return;
+ }
+
+ m_solverType = type;
+ }
+
///
/// \brief Solve the non linear system of equations G(x)=0 using Newton's method.
///
@@ -99,12 +120,14 @@ public:
private:
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<StdVectorOfVec3d> m_positions = nullptr;
std::shared_ptr<StdVectorOfReal> m_invMasses = nullptr;
+ PbdConstraint::SolverType m_solverType = PbdConstraint::SolverType::xPBD;
};
class PbdCollisionSolver : SolverBase