diff --git a/Examples/PBD/PBDCloth/pbdClothExample.cpp b/Examples/PBD/PBDCloth/pbdClothExample.cpp
index 4103f8855d3f63dbd7156e29990fa4c9722328ce..bfbcc57d17e1dc1521cc5f8dc9bc7043584041e5 100644
--- a/Examples/PBD/PBDCloth/pbdClothExample.cpp
+++ b/Examples/PBD/PBDCloth/pbdClothExample.cpp
@@ -104,7 +104,7 @@ main()
// 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};
+ std::vector<size_t> fixedNodes = { 0, nCols - 1 };
pbdParams->m_fixedNodeIds = fixedNodes;
// Other parameters
diff --git a/Examples/PBD/PBDString/pbdStringExample.cpp b/Examples/PBD/PBDString/pbdStringExample.cpp
index 280995dd0ba2ba6d46f4be66d3bc1947c43ec0be..a6b8e2b8a7d1e846c6dd0a94d3daeb8a52c2fd11 100644
--- a/Examples/PBD/PBDString/pbdStringExample.cpp
+++ b/Examples/PBD/PBDString/pbdStringExample.cpp
@@ -92,7 +92,7 @@ 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, 1e7);
- sims[i].params->enableConstraint(PbdConstraint::Type::Bend, (static_cast<double>(i) * 0.1 / numStrings + 0.001)*1e6);
+ 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 6fb73476d848b707d2559ff20be030b37231b616..6121b15e54634b7d15e52f26976341c327a21ed0 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.cpp
@@ -34,7 +34,7 @@ PbdAreaConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
m_vertexIds[1] = pIdx1;
m_vertexIds[2] = pIdx2;
- this->m_stiffness = k;
+ this->m_stiffness = k;
this->m_compliance = 1.0 / k;
const Vec3d& p0 = initVertexPositions[pIdx0];
@@ -62,9 +62,12 @@ PbdAreaConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPos
const Vec3d e2 = p2 - p0;
Vec3d n = e0.cross(e1);
- c = 0.5 * n.norm();
+ c = 0.5 * n.norm();
- if (c < m_epsilon) return false;
+ if (c < m_epsilon)
+ {
+ return false;
+ }
n /= 2 * c;
c -= m_restArea;
diff --git a/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h
index 303643d68016c6cb7040b6e4041c2f1bc42cfd4e..0135fe574b3849b46d1e47ee1cd982b4cf252e32 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdAreaConstraint.h
@@ -52,10 +52,10 @@ public:
const double k = 2.5);
bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
- double& c,
- StdVectorOfVec3d& dcdx) const override;
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
- double m_restArea = 0.; ///> Area at the rest position
+ 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 d4812e18ee995051c3faf07e89f3ce5d3063091c..9bb0315a17ef4617bd539746ca0feeda88b6947f 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.cpp
@@ -24,17 +24,17 @@
namespace imstk
{
void
- PbdBendConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx0,
- const size_t& pIdx1,
- const size_t& pIdx2,
- 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] = pIdx0;
m_vertexIds[1] = pIdx1;
m_vertexIds[2] = pIdx2;
- m_stiffness = k;
+ m_stiffness = k;
m_compliance = 1.0 / k;
const Vec3d& p0 = initVertexPositions[pIdx0];
@@ -49,8 +49,8 @@ void
bool
PbdBendConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
- double& c,
- StdVectorOfVec3d& dcdx) const
+ double& c,
+ StdVectorOfVec3d& dcdx) const
{
const size_t i0 = m_vertexIds[0];
const size_t i1 = m_vertexIds[1];
@@ -65,7 +65,7 @@ PbdBendConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPos
const Vec3d& diff = p1 - center;
const double dist = diff.norm();
- if (dist < m_epsilon)
+ if (dist < m_epsilon)
{
return false;
}
@@ -79,5 +79,4 @@ PbdBendConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPos
return true;
}
-
} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h
index 98c536a5873b6cf45c81958e6cf24f242da0d60f..c2e68dc2f8702512a4f14127834fe361994831f2 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdBendConstraint.h
@@ -67,8 +67,8 @@ public:
/// \brief Compute the value and gradient of constraint
///
bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPosition,
- double& c,
- StdVectorOfVec3d& dcdx) const override;
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
double m_restLength = 0.; ///> Rest length
};
diff --git a/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp
index 9cd0fd6ffb23e9874b032464977a9e1208f3c4db..dda274ce9101a8d8b14404fd0ee0bd6062378fa5 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp
@@ -30,34 +30,37 @@ PbdCollisionConstraint::PbdCollisionConstraint(const unsigned int& n1, const uns
m_bodiesSecond.resize(n2);
}
-
-void PbdCollisionConstraint::projectConstraint(const StdVectorOfReal& invMassA,
- const StdVectorOfReal& invMassB,
- StdVectorOfVec3d& posA,
- StdVectorOfVec3d& posB)
+void
+PbdCollisionConstraint::projectConstraint(const StdVectorOfReal& invMassA,
+ const StdVectorOfReal& invMassB,
+ StdVectorOfVec3d& posA,
+ StdVectorOfVec3d& posB)
{
- double c;
+ double c;
StdVectorOfVec3d dcdxA;
StdVectorOfVec3d dcdxB;
bool update = this->computeValueAndGradient(posA, posB, c, dcdxA, dcdxB);
- if (!update) return;
+ if (!update)
+ {
+ return;
+ }
double lambda = 0.0;
- for (size_t i=0; i<m_bodiesFirst.size(); ++i)
+ 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)
+ 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)
+ for (size_t i = 0, vid = 0; i < m_bodiesFirst.size(); ++i)
{
vid = m_bodiesFirst[i];
if (invMassA[vid] > 0.0)
@@ -66,7 +69,7 @@ void PbdCollisionConstraint::projectConstraint(const StdVectorOfReal& invMassA,
}
}
- for (size_t i=0, vid=0; i<m_bodiesSecond.size(); ++i)
+ for (size_t i = 0, vid = 0; i < m_bodiesSecond.size(); ++i)
{
vid = m_bodiesSecond[i];
if (invMassB[vid] > 0.0)
diff --git a/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h
index 48bc64f3cdad69a2bb3369cf1bc7d512c9bb58b5..bd5270f7224acfaa8362fa49001b2e5bac916a5a 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdCollisionConstraint.h
@@ -70,12 +70,12 @@ public:
///
/// \brief Get config
///
- const PbdCollisionConstraintConfig& getConfigFirst() const {return *m_configA;}
- const PbdCollisionConstraintConfig& getConfigSecond() const {return *m_configB;}
+ 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
@@ -83,14 +83,14 @@ public:
///
virtual bool computeValueAndGradient(const StdVectorOfVec3d& posA,
const StdVectorOfVec3d& posB,
- double& c,
- StdVectorOfVec3d& dcdxA,
- StdVectorOfVec3d& dcdxB) const = 0;
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const = 0;
- virtual void projectConstraint(const StdVectorOfReal& invMassA,
+ virtual void projectConstraint(const StdVectorOfReal& invMassA,
const StdVectorOfReal& invMassB,
- StdVectorOfVec3d& posA,
- StdVectorOfVec3d& posB);
+ 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 e8dca06850d267252431be0fbb52f8ca500a0abe..03fe3fd841019825d024481ad38f58ffb441640d 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.cpp
@@ -45,10 +45,10 @@ PbdConstantDensityConstraint::initConstraint(const StdVectorOfVec3d& initVertexP
}
void
-PbdConstantDensityConstraint::projectConstraint(const StdVectorOfReal& currInvMasses,
- const double dt,
+PbdConstantDensityConstraint::projectConstraint(const StdVectorOfReal& currInvMasses,
+ const double dt,
const PbdConstraint::SolverType& type,
- StdVectorOfVec3d& currVertexPositions)
+ StdVectorOfVec3d& currVertexPositions)
{
const size_t numParticles = currVertexPositions.size();
diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h
index 44057a36280ddff5388168ab9a46425038cac445..81f97b656f2a54f98b8c07ad9640af60e30f50e6 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h
@@ -59,14 +59,14 @@ public:
///
/// \brief Solves the constant density constraint
///
- void projectConstraint(const StdVectorOfReal& currInvMasses,
- const double dt,
+ void projectConstraint(const StdVectorOfReal& currInvMasses,
+ const double dt,
const PbdConstraint::SolverType& type,
- StdVectorOfVec3d& currVertexPositions) override;
+ StdVectorOfVec3d& currVertexPositions) override;
bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
- double& c,
- StdVectorOfVec3d& dcdx) const
+ double& c,
+ StdVectorOfVec3d& dcdx) const
{
return true;
}
diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdConstraint.cpp
index cb744539329442c798c7ec6f96d8d96b5f2cc070..443c2a17ad140350c8ed738534e314f6a40b1d09 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstraint.cpp
@@ -1,21 +1,45 @@
+/*=========================================================================
+
+ 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)
+void
+PbdConstraint::projectConstraint(const StdVectorOfReal& invMasses, const double dt, const SolverType& solverType, StdVectorOfVec3d& pos)
{
- double c;
+ double c;
StdVectorOfVec3d dcdx;
bool update = this->computeValueAndGradient(pos, c, dcdx);
- if (!update) return;
+ if (!update)
+ {
+ return;
+ }
double dcMidc = 0.0;
double lambda = 0.0;
double alpha;
- for (size_t i=0; i<m_vertexIds.size(); ++i)
+ for (size_t i = 0; i < m_vertexIds.size(); ++i)
{
dcMidc += invMasses[m_vertexIds[i]] * dcdx[i].squaredNorm();
}
@@ -28,20 +52,20 @@ void PbdConstraint::projectConstraint(const StdVectorOfReal& invMasses, const do
switch (solverType)
{
case (SolverType::xPBD):
- alpha = m_compliance / (dt * dt );
- lambda = -(c + alpha * m_lambda) / (dcMidc + alpha);
+ 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);
+ 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)
+ for (size_t i = 0, vid = 0; i < m_vertexIds.size(); ++i)
{
vid = m_vertexIds[i];
if (invMasses[vid] > 0.0)
@@ -52,5 +76,4 @@ void PbdConstraint::projectConstraint(const StdVectorOfReal& invMasses, const do
return;
}
-
}
diff --git a/Source/Constraint/PbdConstraints/imstkPbdConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
index ffdc36ac0a1b8d48c7765ce3d04e46ada3c531f1..927fc9eeffc0e5e0172a29a619997a5fd97e0883 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdConstraint.h
@@ -71,7 +71,7 @@ public:
///
virtual Type getType() const = 0;
- ///
+ ///
/// \brief Compute value and gradient of the constraint
///
/// \param[in] currVertexPositions vector of current positions
@@ -79,8 +79,8 @@ public:
/// \param[inout] dcdx constraint gradient
///
virtual bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
- double& c,
- StdVectorOfVec3d& dcdx) const = 0;
+ double& c,
+ StdVectorOfVec3d& dcdx) const = 0;
///
/// \brief Get the vertex indices of the constraint
@@ -100,9 +100,10 @@ public:
///
/// \brief Set the stiffness
///
- void setStiffness(const double stiffness) {
- m_stiffness = stiffness;
- m_compliance = 1.0 / stiffness;
+ void setStiffness(const double stiffness)
+ {
+ m_stiffness = stiffness;
+ m_compliance = 1.0 / stiffness;
}
///
@@ -111,7 +112,7 @@ public:
double getStiffness() const { return m_stiffness; }
///
- /// \brief Use PBD
+ /// \brief Use PBD
///
void zeroOutLambda() { m_lambda = 0.0; }
@@ -121,9 +122,9 @@ public:
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_stiffness = 1.0; ///> used in PBD, [0, 1]
- double m_compliance = 1e-7; ///> used in xPBD, inverse of Young's Modulus
+ 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
};
diff --git a/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp
index 5b961e489509d727b3237f4adaeb3d6aa2ad15f9..d45e2edd87341aa2e7d24fe589cef5e3411168b4 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.cpp
@@ -26,18 +26,18 @@ namespace imstk
{
void
PbdDihedralConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx0,
- const size_t& pIdx1,
- const size_t& pIdx2,
- const size_t& pIdx3,
- 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] = 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[pIdx0];
@@ -53,8 +53,8 @@ PbdDihedralConstraint::initConstraint(const StdVectorOfVec3d& initVertexPosition
bool
PbdDihedralConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
- double& c,
- StdVectorOfVec3d& dcdx) const
+ double& c,
+ StdVectorOfVec3d& dcdx) const
{
const auto i0 = m_vertexIds[0];
const auto i1 = m_vertexIds[1];
@@ -91,9 +91,8 @@ PbdDihedralConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVerte
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;
- c = atan2(n1.cross(n2).dot(e), l * n1.dot(n2)) - m_restAngle;
+ c = atan2(n1.cross(n2).dot(e), l * n1.dot(n2)) - m_restAngle;
return true;
}
} // imstk
-
diff --git a/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h
index 5592ff0c3dc5d10b054da86b31ed9890d4f308cf..98ad174c203726cf03ff14263504481b79aa5ca4 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdDihedralConstraint.h
@@ -65,7 +65,7 @@ public:
const size_t& pIdx2, const size_t& pIdx3,
const double k);
- ///
+ ///
/// \brief Compute value and gradient of the constraint
///
/// \param[in] currVertexPositions vector of current positions
@@ -73,8 +73,8 @@ public:
/// \param[inout] dcdx constraint gradient
///
bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
- double& c,
- StdVectorOfVec3d& dcdx) const override;
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
double m_restAngle = 0.0; ///> Rest angle
diff --git a/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp
index 408dc3474ee5e9fe53d12555a3bc083b0a511397..f0815882793e062cb43330b643a06f8264a4bf77 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.cpp
@@ -24,15 +24,15 @@
namespace imstk
{
void
-PbdDistanceConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx0,
- const size_t& pIdx1,
- const double k)
+PbdDistanceConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const double k)
{
m_vertexIds[0] = pIdx0;
m_vertexIds[1] = pIdx1;
m_stiffness = k;
- m_compliance = 1.0 / k;
+ m_compliance = 1.0 / k;
const Vec3d& p0 = initVertexPositions[pIdx0];
const Vec3d& p1 = initVertexPositions[pIdx1];
@@ -41,9 +41,9 @@ PbdDistanceConstraint::initConstraint(const StdVectorOfVec3d& initVertexPosition
}
bool
-PbdDistanceConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
- double& c,
- StdVectorOfVec3d& dcdx) const
+PbdDistanceConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
{
const Vec3d& p0 = currVertexPositions[m_vertexIds[0]];
const Vec3d& p1 = currVertexPositions[m_vertexIds[1]];
@@ -52,8 +52,8 @@ PbdDistanceConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVerte
dcdx[0] = p0 - p1;
const double len = dcdx[0].norm();
dcdx[0] /= len;
- dcdx[1] = -dcdx[0];
- c = len - m_restLength;
+ dcdx[1] = -dcdx[0];
+ c = len - m_restLength;
return true;
}
diff --git a/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h
index 81fb8df00ec5e313f016554920da11ca7d3a8ce8..12a7516718723d43461778b97f7ef90b6c3be1b5 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdDistanceConstraint.h
@@ -47,14 +47,13 @@ public:
/// \brief Initializes the distance constraint
///
void initConstraint(const StdVectorOfVec3d& initVertexPositions,
- const size_t& pIdx0,
- const size_t& pIdx1,
- const double k = 1e5);
+ const size_t& pIdx0,
+ const size_t& pIdx1,
+ const double k = 1e5);
bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
- double& c,
- StdVectorOfVec3d& dcdx) const override;
-
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
public:
double m_restLength = 0.0; ///> Rest length between the nodes
diff --git a/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp
index 5c2185e1c1ff0fda99c04fa604afd9571c7634c6..85986e9963e3690f7977c2f6e1854dd387c8b30f 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.cpp
@@ -25,10 +25,10 @@
namespace imstk
{
void
-PbdEdgeEdgeConstraint::initConstraint(const size_t& pIdxA1,
- const size_t& pIdxA2,
- const size_t& pIdxB1,
- const size_t& pIdxB2,
+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)
{
@@ -40,11 +40,12 @@ PbdEdgeEdgeConstraint::initConstraint(const size_t& pIdxA1,
m_bodiesSecond[1] = pIdxB2;
}
-bool PbdEdgeEdgeConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
- const StdVectorOfVec3d& currVertexPositionsB,
- double& cc,
- StdVectorOfVec3d& dcdxA,
- StdVectorOfVec3d& dcdxB) const
+bool
+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];
diff --git a/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
index 9679f281a89486b5ee912d3b1714f16bb3705add..74b519b35c10956bd23f0e0c7a7e456bf0c61af4 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.h
@@ -54,7 +54,7 @@ public:
///
/// \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
@@ -62,9 +62,8 @@ public:
///
bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
const StdVectorOfVec3d& currVertexPositionsB,
- double& c,
- StdVectorOfVec3d& dcdxA,
- StdVectorOfVec3d& dcdxB) const override;
-
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const override;
};
} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp
index 8006c93934c068cb11ef0cc2fc7bd289100e48a6..4763b2f29c6d5d94db7561aec5eac5eb262a230d 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.cpp
@@ -43,7 +43,7 @@ PbdFEMTetConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
m_elementVolume = (1.0 / 6.0) * (p3 - p0).dot((p1 - p0).cross(p2 - p0));
m_config = config;
- m_compliance = 1.0 / ( config->m_lambda + 2*config->m_mu );
+ m_compliance = 1.0 / (config->m_lambda + 2 * config->m_mu);
Mat3d m;
m.col(0) = p0 - p3;
@@ -60,9 +60,10 @@ PbdFEMTetConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
return false;
}
-bool PbdFEMTetConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
- double& cval,
- StdVectorOfVec3d& dcdx) const
+bool
+PbdFEMTetConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& cval,
+ StdVectorOfVec3d& dcdx) const
{
const auto i0 = m_vertexIds[0];
const auto i1 = m_vertexIds[1];
@@ -73,7 +74,7 @@ bool PbdFEMTetConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVe
const Vec3d& p1 = currVertexPositions[i1];
const Vec3d& p2 = currVertexPositions[i2];
const Vec3d& p3 = currVertexPositions[i3];
-
+
Mat3d m;
m.col(0) = p0 - p3;
m.col(1) = p1 - p3;
@@ -174,7 +175,7 @@ bool PbdFEMTetConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVe
}
Mat3d gradC = m_elementVolume * P * m_invRestMat.transpose();
- cval = C;
+ cval = C;
cval *= m_elementVolume;
dcdx.resize(m_vertexIds.size());
dcdx[0] = gradC.col(0);
diff --git a/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h
index 328890ec632723764aed009cce63eadb2bcb86f3..b2dc8e3d2e0111e9884f275844c7b15f216e0084 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdFETetConstraint.h
@@ -57,7 +57,7 @@ public:
/// \brief Compute the value and gradient of constraint
///
bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPosition,
- double& c,
- StdVectorOfVec3d& dcdx) const override;
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
};
} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp
index 4058829738e011e765189dcfb62ec3db4f69b5b7..75b13a8d9e49a1a6ab776c8a3cd7b9e55464758a 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.cpp
@@ -26,10 +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)
{
@@ -44,9 +44,9 @@ PbdPointTriangleConstraint::initConstraint(const size_t& pIdxA1,
bool
PbdPointTriangleConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
const StdVectorOfVec3d& currVertexPositionsB,
- double& c,
- StdVectorOfVec3d& dcdxA,
- StdVectorOfVec3d& dcdxB) const
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const
{
const size_t i0 = m_bodiesFirst[0];
const size_t i1 = m_bodiesSecond[0];
diff --git a/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
index dae77f5630f20dd61da92ef9e5e34cf5bdb9fabf..dc6186775c3ce99bc1fa00956c3ff39a6a5c7631 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdPointTriCollisionConstraint.h
@@ -57,7 +57,7 @@ public:
///
/// \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
@@ -65,8 +65,8 @@ public:
///
bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPositionsA,
const StdVectorOfVec3d& currVertexPositionsB,
- double& c,
- StdVectorOfVec3d& dcdxA,
- StdVectorOfVec3d& dcdxB) const override;
+ double& c,
+ StdVectorOfVec3d& dcdxA,
+ StdVectorOfVec3d& dcdxB) const override;
};
} // imstk
diff --git a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
index 122622774eaeb460a3db3f7d05de4e8962f0ead1..ef63215ebcfa6e58be138a90c170eb9f9012941f 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
+++ b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.cpp
@@ -34,7 +34,7 @@ PbdVolumeConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
m_vertexIds[2] = pIdx2;
m_vertexIds[3] = pIdx3;
- m_stiffness = k;
+ m_stiffness = k;
m_compliance = 1.0 / k;
const Vec3d& p0 = initVertexPositions[pIdx0];
@@ -45,9 +45,10 @@ PbdVolumeConstraint::initConstraint(const StdVectorOfVec3d& initVertexPositions,
m_restVolume = (1.0 / 6.0) * ((p1 - p0).cross(p2 - p0)).dot(p3 - p0);
}
-bool PbdVolumeConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
- double& c,
- StdVectorOfVec3d& dcdx) const
+bool
+PbdVolumeConstraint::computeValueAndGradient(const StdVectorOfVec3d& currVertexPositions,
+ double& c,
+ StdVectorOfVec3d& dcdx) const
{
const auto i0 = m_vertexIds[0];
const auto i1 = m_vertexIds[1];
diff --git a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h
index 7a68d74e3493262f313e57f4bd0902435d49addf..7f92dc4eba9a35b8e006476e9628dd1950300daa 100644
--- a/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h
+++ b/Source/Constraint/PbdConstraints/imstkPbdVolumeConstraint.h
@@ -55,8 +55,8 @@ public:
/// \brief Compute the value and gradient of constraint
///
bool computeValueAndGradient(const StdVectorOfVec3d& currVertexPosition,
- double& c,
- StdVectorOfVec3d& dcdx) const override;
+ double& c,
+ StdVectorOfVec3d& dcdx) const override;
protected:
double m_restVolume = 0.0; ///> Rest volume
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp b/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
index 762910eeb7e0f0326b06f43b7249cab787293909..82a234009575024483a4251d8aa5a8e39ecc0f5b 100644
--- a/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
@@ -146,7 +146,6 @@ PbdModel::initialize()
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";
}
@@ -514,7 +513,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)
{
@@ -542,7 +544,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());
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdModel.h b/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
index d962c6a0f578e7fdf57ded189ae6b270e45517f0..d19efa0302f7c6845477f3b88d3b27735f68c3b3 100644
--- a/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
@@ -59,10 +59,10 @@ struct PBDModelConfig
std::shared_ptr<PbdFEMConstraintConfig> m_femParams =
std::make_shared<PbdFEMConstraintConfig>(PbdFEMConstraintConfig
{
- 0.0, // Lame constant, if constraint type is FEM
- 0.0, // Lame constant, if constraint type is FEM
- 1000.0, // FEM parameter, if constraint type is FEM
- 0.2 // FEM parameter, if constraint type is FEM
+ 0.0, // Lame constant, if constraint type is FEM
+ 0.0, // Lame constant, if constraint type is FEM
+ 1000.0, // FEM parameter, if constraint type is FEM
+ 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
@@ -281,7 +281,7 @@ protected:
void initGraphEdges(std::shared_ptr<ComputeNode> source, std::shared_ptr<ComputeNode> sink) override;
protected:
- bool m_partitioned = false; /// \todo this is used in initialize() as a temporary fix to problems on linux
+ 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
diff --git a/Source/Solvers/imstkPbdSolver.cpp b/Source/Solvers/imstkPbdSolver.cpp
index c3cf20071349b081a9fb7f6964f2b6af4cdab770..a03ff5351351d8448fa6371394dc1600eaaffe29 100644
--- a/Source/Solvers/imstkPbdSolver.cpp
+++ b/Source/Solvers/imstkPbdSolver.cpp
@@ -52,8 +52,8 @@ PbdSolver::solve()
{
const PBDConstraintVector& constraintPartition = partitionedConstraints[j];
ParallelUtils::parallelFor(constraintPartition.size(),
- [&](const size_t idx) { constraintPartition[idx]->zeroOutLambda(); }
- );
+ [&](const size_t idx) { constraintPartition[idx]->zeroOutLambda(); }
+ );
}
unsigned int i = 0;
@@ -66,7 +66,7 @@ PbdSolver::solve()
for (size_t j = 0; j < partitionedConstraints.size(); j++)
{
- const PBDConstraintVector& constraintPartition = partitionedConstraints[j];
+ const PBDConstraintVector& constraintPartition = partitionedConstraints[j];
ParallelUtils::parallelFor(constraintPartition.size(),
[&](const size_t idx)
diff --git a/Source/Solvers/imstkPbdSolver.h b/Source/Solvers/imstkPbdSolver.h
index 8d7f61d748409bc15fdbfea835536fbdd17f64e0..f7012d6b5766bd9e6315db03f85812299793909b 100644
--- a/Source/Solvers/imstkPbdSolver.h
+++ b/Source/Solvers/imstkPbdSolver.h
@@ -120,7 +120,7 @@ public:
private:
size_t m_iterations = 20; ///> Number of NL Gauss-Seidel iterations for regular constraints
- double m_dt; ///> time step
+ 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