diff --git a/Base/Constraint/imstkPbdAreaConstraint.cpp b/Base/Constraint/imstkPbdAreaConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f1726b24d6eb82999b3d4b65cdfb820f3b78ca2b
--- /dev/null
+++ b/Base/Constraint/imstkPbdAreaConstraint.cpp
@@ -0,0 +1,106 @@
+/*=========================================================================
+
+ 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 "imstkPbdAreaConstraint.h"
+#include "imstkPbdModel.h"
+
+namespace imstk
+{
+
+void
+AreaConstraint::initConstraint(PositionBasedModel &model, const unsigned int &pIdx1,
+ const unsigned int &pIdx2, const unsigned int &pIdx3,
+ const double k)
+{
+ m_bodies[0] = pIdx1;
+ m_bodies[1] = pIdx2;
+ m_bodies[2] = pIdx3;
+
+ m_stiffness = k;
+
+ auto state = model.getState();
+
+ const Vec3d &p0 = state->getInitialVertexPosition(pIdx1);
+ const Vec3d &p1 = state->getInitialVertexPosition(pIdx2);
+ const Vec3d &p2 = state->getInitialVertexPosition(pIdx3);
+
+ m_restArea = 0.5*(p1 - p0).cross(p2 - p0).norm();
+}
+
+bool
+AreaConstraint::solvePositionConstraint(PositionBasedModel &model)
+{
+ const unsigned int i1 = m_bodies[0];
+ const unsigned int i2 = m_bodies[1];
+ const unsigned int i3 = m_bodies[2];
+
+ auto state = model.getState();
+
+ Vec3d &p0 = state->getVertexPosition(i1);
+ Vec3d &p1 = state->getVertexPosition(i2);
+ Vec3d &p2 = state->getVertexPosition(i3);
+
+ const double im0 = state->getInvMass(i1);
+ const double im1 = state->getInvMass(i2);
+ const double im2 = state->getInvMass(i3);
+
+ const Vec3d e1 = p0 - p1;
+ const Vec3d e2 = p1 - p2;
+ const Vec3d e3 = p2 - p0;
+
+ Vec3d n = e1.cross(e2);
+ const double A = 0.5*n.norm();
+
+ if (A < EPS)
+ {
+ return false;
+ }
+
+ n /= 2 * A;
+
+ const Vec3d grad0 = e2.cross(n);
+ const Vec3d grad1 = e3.cross(n);
+ const Vec3d grad2 = e1.cross(n);
+
+ double 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;
+ }
+
+ return true;
+}
+
+
+} // imstk
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdAreaConstraint.h b/Base/Constraint/imstkPbdAreaConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..a6538c442d7a3e653eb886d0047727d740331bc2
--- /dev/null
+++ b/Base/Constraint/imstkPbdAreaConstraint.h
@@ -0,0 +1,68 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef IMSTK_PBD_AREA_CONSTRAINT_H
+#define IMSTK_PBD_AREA_CONSTRAINT_H
+
+#include "imstkPbdConstraint.h"
+
+namespace imstk
+{
+
+////
+/// \class AreaConstraint
+///
+/// \brief Area constraint for triangular face
+///
+class AreaConstraint : public PbdConstraint
+{
+public:
+ ///
+ /// \brief Constructor
+ ///
+ AreaConstraint() : PbdConstraint(3) {}
+
+ ///
+ /// \brief Returns PBD constraint of type Type::Area
+ ///
+ Type getType() const
+ {
+ return Type::Area;
+ }
+
+ ///
+ /// \brief Initializes the area constraint
+ ///
+ void initConstraint(PositionBasedModel& model, const unsigned int& pIdx1,
+ const unsigned int& pIdx2, const unsigned int& pIdx3, const double k = 2.5);
+
+ ///
+ /// \brief Solves the area constraint
+ ///
+ bool solvePositionConstraint(PositionBasedModel &model);
+
+public:
+ double m_restArea; ///> Area at the rest position
+ double m_stiffness; ///> Stiffness of the area constraint
+};
+}
+
+#endif // IMSTK_PBD_AREA_CONSTRAINT_H
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdConstraint.cpp b/Base/Constraint/imstkPbdConstraint.cpp
deleted file mode 100644
index 320d6d46d95b7bf1424c0e0ce06fa4241e008d00..0000000000000000000000000000000000000000
--- a/Base/Constraint/imstkPbdConstraint.cpp
+++ /dev/null
@@ -1,484 +0,0 @@
-/*=========================================================================
-
- 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"
-#include "imstkPbdModel.h"
-
-namespace imstk
-{
-
-using Vec3d = Eigen::Vector3d;
-using Mat3d = Eigen::Matrix3d;
-
-bool
-FEMTetConstraint::initConstraint (PositionBasedModel &model,
- const unsigned int &pIdx1, const unsigned int &pIdx2,
- const unsigned int &pIdx3, const unsigned int &pIdx4)
-{
- m_bodies[0] = pIdx1;
- m_bodies[1] = pIdx2;
- m_bodies[2] = pIdx3;
- m_bodies[3] = pIdx4;
-
- auto state = model.getState();
-
- Vec3d &p0 = state->getInitialVertexPosition(pIdx1);
- Vec3d &p1 = state->getInitialVertexPosition(pIdx2);
- Vec3d &p2 = state->getInitialVertexPosition(pIdx3);
- Vec3d &p3 = state->getInitialVertexPosition(pIdx4);
-
- m_Volume = (1.0 / 6.0) * (p3 - p0).dot((p1 - p0).cross(p2 - p0));
-
- Mat3d m;
- m.col(0) = p0 - p3;
- m.col(1) = p1 - p3;
- m.col(2) = p2 - p3;
-
- double det = m.determinant();
- if (fabs(det) > EPS)
- {
- m_invRestMat = m.inverse();
- return true;
- }
-
- return false;
-}
-
-bool
-FEMTetConstraint::solvePositionConstraint(PositionBasedModel &model)
-{
- const unsigned int i1 = m_bodies[0];
- const unsigned int i2 = m_bodies[1];
- const unsigned int i3 = m_bodies[2];
- const unsigned int i4 = m_bodies[3];
-
- auto state = model.getState();
-
- Vec3d &p0 = state->getVertexPosition(i1);
- Vec3d &p1 = state->getVertexPosition(i2);
- Vec3d &p2 = state->getVertexPosition(i3);
- Vec3d &p3 = state->getVertexPosition(i4);
-
- double currentVolume = (1.0 / 6.0) * (p3 - p0).dot((p1 - p0).cross(p2 - p0));
-
- Mat3d m;
- m.col(0) = p0 - p3;
- m.col(1) = p1 - p3;
- m.col(2) = p2 - p3;
-
- // deformation gradient
- Mat3d F = m*m_invRestMat;
- // First Piola-Kirchhoff tensor
- Mat3d P;
- // energy constraint
- double C = 0;
-
- double mu = model.getFirstLame();
- double lambda = model.getSecondLame();
-
- switch (m_material)
- {
-
- // P(F) = F*(2*mu*E + lambda*tr(E)*I)
- // E = (F^T*F - I)/2
- case MaterialType::StVK :
- {
- Mat3d E;
- E(0, 0) = 0.5*(F(0, 0) * F(0, 0) + F(1, 0) * F(1, 0) + F(2, 0) * F(2, 0) - 1.0); // xx
- E(1, 1) = 0.5*(F(0, 1) * F(0, 1) + F(1, 1) * F(1, 1) + F(2, 1) * F(2, 1) - 1.0); // yy
- E(2, 2) = 0.5*(F(0, 2) * F(0, 2) + F(1, 2) * F(1, 2) + F(2, 2) * F(2, 2) - 1.0); // zz
- E(0, 1) = 0.5*(F(0, 0) * F(0, 1) + F(1, 0) * F(1, 1) + F(2, 0) * F(2, 1)); // xy
- E(0, 2) = 0.5*(F(0, 0) * F(0, 2) + F(1, 0) * F(1, 2) + F(2, 0) * F(2, 2)); // xz
- E(1, 2) = 0.5*(F(0, 1) * F(0, 2) + F(1, 1) * F(1, 2) + F(2, 1) * F(2, 2)); // yz
- E(1, 0) = E(0, 1);
- E(2, 0) = E(0, 2);
- E(2, 1) = E(1, 2);
-
- P = 2*mu*E;
- double tr = E.trace();
- double lt = lambda*tr;
- P(0,0) += lt;
- P(1,1) += lt;
- P(2,2) += lt;
- P = F*P;
-
- C = E(0,0)*E(0,0) + E(0,1)*E(0,1) + E(0,2)*E(0,2)
- + E(1,0)*E(1,0) + E(1,1)*E(1,1) + E(1,2)*E(1,2)
- + E(2,0)*E(2,0) + E(2,1)*E(2,1) + E(2,2)*E(2,2);
- C = mu*C + 0.5*lambda*tr*tr;
-
- break;
- }
-
- // P(F) = (2*mu*(F-R) + lambda*(J-1)*J*F^-T
- case MaterialType::Corotation :
- {
- Eigen::JacobiSVD<Mat3d> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
- Mat3d R = svd.matrixU()*svd.matrixV().adjoint();
- Vec3d Sigma(svd.singularValues());
- Mat3d invFT = svd.matrixU();
- invFT.col(0) /= Sigma(0);
- invFT.col(1) /= Sigma(1);
- invFT.col(2) /= Sigma(2);
- invFT *= svd.matrixV().adjoint();
- double J = Sigma(0)*Sigma(1)*Sigma(2);
- Mat3d FR = F - R;
-
- P = 2*mu*FR + lambda*(J-1)*J*invFT;
-
- C = FR(0,0)*FR(0,0) + FR(0,1)*FR(0,1) + FR(0,2)*FR(0,2)
- + FR(1,0)*FR(1,0) + FR(1,1)*FR(1,1) + FR(1,2)*FR(1,2)
- + FR(2,0)*FR(2,0) + FR(2,1)*FR(2,1) + FR(2,2)*FR(2,2);
- C = mu*C + 0.5*lambda*(J-1)*(J-1);
-
- break;
- }
- // P(F) = mu*(F - mu*F^-T) + lambda*log(J)F^-T;
- case MaterialType::NeoHookean :
- {
- Mat3d invFT = F.inverse().transpose();
- double logJ = log(F.determinant());
- P = mu*(F - invFT) + lambda*logJ*invFT;
-
- C = F(0,0)*F(0,0) + F(0,1)*F(0,1) + F(0,2)*F(0,2)
- + F(1,0)*F(1,0) + F(1,1)*F(1,1) + F(1,2)*F(1,2)
- + F(2,0)*F(2,0) + F(2,1)*F(2,1) + F(2,2)*F(2,2);
-
- C = 0.5*mu*(C-3) - mu*logJ + 0.5*lambda*logJ*logJ;
-
- break;
- }
-
- case MaterialType::Linear :
- {
- break;
- }
-
- default: {
- printf("Material type not supported ! \n") ;
- break;
- }
- }
-
- const double im1 = state->getInvMass(i1);
- const double im2 = state->getInvMass(i2);
- const double im3 = state->getInvMass(i3);
- const double im4 = state->getInvMass(i4);
-
- Mat3d gradC = m_Volume*P*m_invRestMat.transpose();
-
- 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 < EPS)
- return false;
-
- C = C*m_Volume;
-
- 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));
-
- return true;
-}
-
-void
-DistanceConstraint::initConstraint(PositionBasedModel &model, const unsigned int &pIdx1,
- const unsigned int &pIdx2, const double k)
-{
- m_bodies[0] = pIdx1;
- m_bodies[1] = pIdx2;
- m_stiffness = k;
- auto state = model.getState();
- Vec3d &p1 = state->getInitialVertexPosition(pIdx1);
- Vec3d &p2 = state->getInitialVertexPosition(pIdx2);
- m_restLength = (p1 - p2).norm();
-}
-
-bool
-DistanceConstraint::solvePositionConstraint(PositionBasedModel &model)
-{
- const unsigned int i1 = m_bodies[0];
- const unsigned int i2 = m_bodies[1];
-
- auto state = model.getState();
-
- Vec3d &p0 = state->getVertexPosition(i1);
- Vec3d &p1 = state->getVertexPosition(i2);
-
- const double im1 = state->getInvMass(i1);
- const double im2 = state->getInvMass(i2);
-
- double wsum = im1 + im2;
-
- if (wsum == 0.0)
- return false;
- Vec3d n = p1 - p0;
- double len = n.norm();
- n /= len;
-
- Vec3d gradC = m_stiffness*n*(len - m_restLength)/wsum;
-
- if (im1 > 0)
- p0 += im1*gradC;
- if (im2 > 0)
- p1 += -im2*gradC;
-
- return true;
-}
-
-void
-DihedralConstraint::initConstraint(PositionBasedModel &model,
- const unsigned int &pIdx1, const unsigned int &pIdx2,
- const unsigned int &pIdx3, const unsigned int &pIdx4,
- const double k)
-{
- m_bodies[0] = pIdx1;
- m_bodies[1] = pIdx2;
- m_bodies[2] = pIdx3;
- m_bodies[3] = pIdx4;
-
- m_stiffness = k;
- auto state = model.getState();
-
- Vec3d &p0 = state->getInitialVertexPosition(pIdx1);
- Vec3d &p1 = state->getInitialVertexPosition(pIdx2);
- Vec3d &p2 = state->getInitialVertexPosition(pIdx3);
- Vec3d &p3 = state->getInitialVertexPosition(pIdx4);
-
- Vec3d n1 = (p2 - p0).cross(p3 - p0).normalized();
- Vec3d n2 = (p3 - p1).cross(p2 - p1).normalized();
-
- m_restAngle = atan2(n1.cross(n2).dot(p3-p2), (p3-p2).norm()*n1.dot(n2));
-}
-
-bool
-DihedralConstraint::solvePositionConstraint(PositionBasedModel &model)
-{
- const unsigned int i1 = m_bodies[0];
- const unsigned int i2 = m_bodies[1];
- const unsigned int i3 = m_bodies[2];
- const unsigned int i4 = m_bodies[3];
-
- auto state = model.getState();
-
- Vec3d &p0 = state->getVertexPosition(i1);
- Vec3d &p1 = state->getVertexPosition(i2);
- Vec3d &p2 = state->getVertexPosition(i3);
- Vec3d &p3 = state->getVertexPosition(i4);
-
- const double im0 = state->getInvMass(i1);
- const double im1 = state->getInvMass(i2);
- const double im2 = state->getInvMass(i3);
- const double im3 = state->getInvMass(i4);
-
- if (im0 == 0.0 && im1 == 0.0)
- return false;
- Vec3d e = p3 - p2;
- Vec3d e1 = p3 - p0;
- Vec3d e2 = p0 - p2;
- Vec3d e3 = p3 - p1;
- Vec3d e4 = p1 - p2;
- // To accelerate, all normal (area) vectors and edge length should be precomputed in parallel
- Vec3d n1 = e1.cross(e);
- Vec3d n2 = e.cross(e3);
- double A1 = n1.norm();
- double A2 = n2.norm();
- n1 /= A1;
- n2 /= A2;
-
- double l = e.norm();
- if ( l < EPS )
- return false;
-
- Vec3d grad0 = -(l/A1)*n1;
- Vec3d grad1 = -(l/A2)*n2;
- Vec3d grad2 = (e.dot(e1)/(A1*l))*n1 + (e.dot(e3)/(A2*l))*n2 ;
- Vec3d grad3 = (e.dot(e2)/(A1*l))*n1 + (e.dot(e4)/(A2*l))*n2 ;
-
- double 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;
- if (im1 > 0)
- p1 += -im1*lambda*grad1;
- if (im2 > 0)
- p2 += -im2*lambda*grad2;
- if (im3 > 0)
- p3 += -im3*lambda*grad3;
-
- return true;
-}
-
-void
-AreaConstraint::initConstraint(PositionBasedModel &model, const unsigned int &pIdx1,
- const unsigned int &pIdx2, const unsigned int &pIdx3,
- const double k)
-{
- m_bodies[0] = pIdx1;
- m_bodies[1] = pIdx2;
- m_bodies[2] = pIdx3;
-
- m_stiffness = k;
-
- auto state = model.getState();
-
- Vec3d &p0 = state->getInitialVertexPosition(pIdx1);
- Vec3d &p1 = state->getInitialVertexPosition(pIdx2);
- Vec3d &p2 = state->getInitialVertexPosition(pIdx3);
-
- m_restArea = 0.5*(p1 - p0).cross(p2 - p0).norm();
-}
-
-bool
-AreaConstraint::solvePositionConstraint(PositionBasedModel &model)
-{
- const unsigned int i1 = m_bodies[0];
- const unsigned int i2 = m_bodies[1];
- const unsigned int i3 = m_bodies[2];
-
- auto state = model.getState();
-
- Vec3d &p0 = state->getVertexPosition(i1);
- Vec3d &p1 = state->getVertexPosition(i2);
- Vec3d &p2 = state->getVertexPosition(i3);
-
- const double im0 = state->getInvMass(i1);
- const double im1 = state->getInvMass(i2);
- const double im2 = state->getInvMass(i3);
-
- Vec3d e1 = p0 - p1;
- Vec3d e2 = p1 - p2;
- Vec3d e3 = p2 - p0;
-
- Vec3d n = e1.cross(e2);
- double A = 0.5*n.norm();
-
- if (A < EPS)
- return false;
-
- n /= 2*A;
-
- Vec3d grad0 = e2.cross(n);
- Vec3d grad1 = e3.cross(n);
- Vec3d grad2 = e1.cross(n);
-
- double 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;
-
- return true;
-}
-
-void
-VolumeConstraint::initConstraint(PositionBasedModel &model, const unsigned int &pIdx1,
- const unsigned int &pIdx2, const unsigned int &pIdx3,
- const unsigned int &pIdx4, const double k)
-{
- m_bodies[0] = pIdx1;
- m_bodies[1] = pIdx2;
- m_bodies[2] = pIdx3;
- m_bodies[3] = pIdx4;
-
- m_stiffness = k;
-
- auto state = model.getState();
-
- Vec3d &p0 = state->getInitialVertexPosition(pIdx1);
- Vec3d &p1 = state->getInitialVertexPosition(pIdx2);
- Vec3d &p2 = state->getInitialVertexPosition(pIdx3);
- Vec3d &p3 = state->getInitialVertexPosition(pIdx4);
-
- m_restVolume = (1.0/6.0)*((p1-p0).cross(p2-p0)).dot(p3-p0);
-}
-
-bool
-VolumeConstraint::solvePositionConstraint(PositionBasedModel &model)
-{
- const unsigned int i1 = m_bodies[0];
- const unsigned int i2 = m_bodies[1];
- const unsigned int i3 = m_bodies[2];
- const unsigned int i4 = m_bodies[3];
-
- auto state = model.getState();
-
- Vec3d &x1 = state->getVertexPosition(i1);
- Vec3d &x2 = state->getVertexPosition(i2);
- Vec3d &x3 = state->getVertexPosition(i3);
- Vec3d &x4 = state->getVertexPosition(i4);
-
- const double im1 = state->getInvMass(i1);
- const double im2 = state->getInvMass(i2);
- const double im3 = state->getInvMass(i3);
- const double im4 = state->getInvMass(i4);
-
- double onesixth = 1.0/6.0;
-
- Vec3d grad1 = onesixth*(x2-x3).cross(x4-x2);
- Vec3d grad2 = onesixth*(x3-x1).cross(x4-x1);
- Vec3d grad3 = onesixth*(x4-x1).cross(x2-x1);
- Vec3d grad4 = onesixth*(x2-x1).cross(x3-x1);
-
- double 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 (im1 > 0)
- x2 += -im2*lambda*grad2;
- if (im3 > 0)
- x3 += -im3*lambda*grad3;
- if (im4 > 0)
- x4 += -im4*lambda*grad4;
-
- return true;
-}
-
-} // imstk
diff --git a/Base/Constraint/imstkPbdConstraint.h b/Base/Constraint/imstkPbdConstraint.h
index 33fd2782e949cf61e6d230a24da369ccca312394..37b6db778e67178ccd5ba50a2e284a96145eeb76 100644
--- a/Base/Constraint/imstkPbdConstraint.h
+++ b/Base/Constraint/imstkPbdConstraint.h
@@ -19,8 +19,8 @@
=========================================================================*/
-#ifndef IMSTKPBDCONSTRAINT_H
-#define IMSTKPBDCONSTRAINT_H
+#ifndef IMSTK_PBD_CONSTRAINT_H
+#define IMSTK_PBD_CONSTRAINT_H
#include "imstkMath.h"
@@ -32,11 +32,14 @@ namespace imstk
class PositionBasedModel;
///
-/// \brief Based Constraint class for Position based dynamics
+/// \brief Base Constraint class for Position based dynamics constraints
///
class PbdConstraint
{
public:
+ ///
+ /// \brief Type of the PBD constraint
+ ///
enum class Type
{
Distance,
@@ -95,268 +98,6 @@ public:
std::vector<unsigned int> m_bodies; // index of points for the constraint
};
-///
-/// \class DistanceConstraint
-///
-/// \brief
-///
-class DistanceConstraint : public PbdConstraint
-{
-public:
- ///
- /// \brief
- ///
- DistanceConstraint() : PbdConstraint(2) {}
-
- ///
- /// \brief
- ///
- Type getType() const { return Type::Distance; }
-
- ///
- /// \brief
- ///
- void initConstraint(PositionBasedModel& model, const unsigned int& pIdx1,
- const unsigned int& pIdx2, const double k = 1e-1);
-
- ///
- /// \brief
- ///
- bool solvePositionConstraint(PositionBasedModel &model);
-
-public:
- double m_restLength;
- double m_stiffness;
-};
-
-///
-/// \class DihedralConstraint
-///
-/// \brief
-///
-class DihedralConstraint : public PbdConstraint
-{
-public:
- ///
- /// \brief
- ///
- DihedralConstraint() : PbdConstraint(4) {}
- ///
- /// \brief
- ///
- Type getType() const { return Type::Dihedral; }
-
- ///
- /// \brief initConstraint
- /// p3
- /// / | \
- /// / | \
- /// p0 | p1
- /// \ | /
- /// \ | /
- /// p2
- /// \param model
- /// \param pIdx1 index of p0
- /// \param pIdx2 index of p1
- /// \param pIdx3 index of p2
- /// \param pIdx4 index of p3
- /// \param k stiffness
- ///
- void initConstraint(PositionBasedModel& model, const unsigned int& pIdx1, const unsigned int& pIdx2,
- const unsigned int& pIdx3, const unsigned int& pIdx4, const double k = 1e-3 );
-
- ///
- /// \brief
- ///
- bool solvePositionConstraint(PositionBasedModel &model);
-
-public:
- double m_restAngle;
- double m_stiffness;
-};
-
-///
-/// \class AreaConstraint
-///
-/// \brief
-///
-class AreaConstraint : public PbdConstraint
-{
-public:
- ///
- /// \brief
- ///
- AreaConstraint() : PbdConstraint(3)
- {}
-
- ///
- /// \brief
- ///
- Type getType() const
- {
- return Type::Area;
- }
-
- ///
- /// \brief
- ///
- void initConstraint(PositionBasedModel& model, const unsigned int& pIdx1,
- const unsigned int& pIdx2, const unsigned int& pIdx3, const double k = 2.5);
-
- ///
- /// \brief
- ///
- bool solvePositionConstraint(PositionBasedModel &model);
-
-public:
- double m_restArea;
- double m_stiffness;
-};
-
-///
-/// \class VolumeConstraint
-///
-/// \brief
-///
-class VolumeConstraint : public PbdConstraint
-{
-public:
- ///
- /// \brief
- ///
- VolumeConstraint() : PbdConstraint(4)
- {}
-
- ///
- /// \brief
- ///
- Type getType() const
- {
- return Type::Volume;
- }
-
- ///
- /// \brief
- ///
- void initConstraint(PositionBasedModel& model, const unsigned int& pIdx1, const unsigned int& pIdx2,
- const unsigned int& pIdx3, const unsigned int& pIdx4, const double k = 2.0 );
-
- ///
- /// \brief
- ///
- bool solvePositionConstraint(PositionBasedModel &model);
-
-public:
- double m_restVolume;
- double m_stiffness;
-};
-
-///
-/// \class FEMConstraint
-///
-/// \brief The FEMConstraint class for constraint as the elastic energy
-/// computed by linear shape functions with tetrahedral mesh.
-/// We provide several model for elastic energy including:
-/// Linear, Corrotation, St Venant-Kirchhof and NeoHookean
-///
-class FEMConstraint : public PbdConstraint
-{
-public:
- enum class MaterialType
- {
- Linear,
- Corotation,
- StVK,
- NeoHookean
- } m_material;
-
- ///
- /// \brief
- ///
- explicit FEMConstraint(const unsigned int nP, MaterialType mtype = MaterialType::StVK) :
- PbdConstraint(nP) , m_material(mtype)
- {}
-
-public:
- double m_Volume;
- Eigen::Matrix3d m_invRestMat;
-};
-
-///
-/// \class FEMTetConstraint
-///
-/// \brief The FEMTetConstraint class class for constraint as the elastic energy
-/// computed by linear shape functions with tetrahedral mesh.
-///
-class FEMTetConstraint : public FEMConstraint
-{
-public:
- ///
- /// \brief
- ///
- explicit FEMTetConstraint( MaterialType mtype = MaterialType::StVK) :
- FEMConstraint(4, mtype)
- {}
-
- ///
- /// \brief
- ///
- Type getType() const
- {
- return Type::FEMTet;
- }
-
- ///
- /// \brief
- ///
- bool initConstraint(PositionBasedModel& model, const unsigned int& pIdx1, const unsigned int& pIdx2,
- const unsigned int& pIdx3, const unsigned int& pIdx4);
-
- ///
- /// \brief
- ///
- bool solvePositionConstraint(PositionBasedModel &model);
-};
-
-///
-/// \class FEMHexConstraint
-///
-/// \brief The FEMHexConstraint class class for constraint as the elastic energy
-/// computed by linear shape functions with hexahedral mesh.
-///
-class FEMHexConstraint : public FEMConstraint
-{
-public:
- ///
- /// \brief
- ///
- explicit FEMHexConstraint( MaterialType mtype = MaterialType::StVK) :
- FEMConstraint(8, mtype)
- {}
-
- ///
- /// \brief
- ///
- Type getType() const
- {
- return Type::FEMHex;
- }
-
- ///
- /// \brief
- ///
- bool initConstraint(PositionBasedModel& model, const unsigned int& pIdx1,
- const unsigned int& pIdx2, const unsigned int& pIdx3,
- const unsigned int& pIdx4, const unsigned int& pIdx5,
- const unsigned int& pIdx6, const unsigned int& pIdx7,
- const unsigned int& pIdx8);
-
- ///
- /// \brief
- ///
- bool solvePositionConstraint(PositionBasedModel &model);
-};
-
}
-#endif // IMSTKPBDCONSTRAINT_H
\ No newline at end of file
+#endif // IMSTK_PBD_CONSTRAINT_H
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdDihedralConstraint.cpp b/Base/Constraint/imstkPbdDihedralConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0124f43623ad73b203ebec6b5a20ae9d63ecc98d
--- /dev/null
+++ b/Base/Constraint/imstkPbdDihedralConstraint.cpp
@@ -0,0 +1,133 @@
+/*=========================================================================
+
+ 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 "imstkPbdDihedralConstraint.h"
+#include "imstkPbdModel.h"
+
+namespace imstk
+{
+
+void
+DihedralConstraint::initConstraint(PositionBasedModel &model,
+ const unsigned int &pIdx1, const unsigned int &pIdx2,
+ const unsigned int &pIdx3, const unsigned int &pIdx4,
+ const double k)
+{
+ m_bodies[0] = pIdx1;
+ m_bodies[1] = pIdx2;
+ m_bodies[2] = pIdx3;
+ m_bodies[3] = pIdx4;
+
+ m_stiffness = k;
+ auto state = model.getState();
+
+ const Vec3d &p0 = state->getInitialVertexPosition(pIdx1);
+ const Vec3d &p1 = state->getInitialVertexPosition(pIdx2);
+ const Vec3d &p2 = state->getInitialVertexPosition(pIdx3);
+ const Vec3d &p3 = state->getInitialVertexPosition(pIdx4);
+
+ const Vec3d n1 = (p2 - p0).cross(p3 - p0).normalized();
+ const Vec3d n2 = (p3 - p1).cross(p2 - p1).normalized();
+
+ m_restAngle = atan2(n1.cross(n2).dot(p3 - p2), (p3 - p2).norm()*n1.dot(n2));
+}
+
+bool
+DihedralConstraint::solvePositionConstraint(PositionBasedModel& model)
+{
+ const unsigned int i1 = m_bodies[0];
+ const unsigned int i2 = m_bodies[1];
+ const unsigned int i3 = m_bodies[2];
+ const unsigned int i4 = m_bodies[3];
+
+ auto state = model.getState();
+
+ Vec3d &p0 = state->getVertexPosition(i1);
+ Vec3d &p1 = state->getVertexPosition(i2);
+ Vec3d &p2 = state->getVertexPosition(i3);
+ Vec3d &p3 = state->getVertexPosition(i4);
+
+ const double im0 = state->getInvMass(i1);
+ const double im1 = state->getInvMass(i2);
+ const double im2 = state->getInvMass(i3);
+ const double im3 = state->getInvMass(i4);
+
+ if (im0 == 0.0 && im1 == 0.0)
+ {
+ return false;
+ }
+
+ const Vec3d e = p3 - p2;
+ const Vec3d e1 = p3 - p0;
+ const Vec3d e2 = p0 - p2;
+ const Vec3d e3 = p3 - p1;
+ const Vec3d e4 = p1 - p2;
+ // To accelerate, all normal (area) vectors and edge length should be precomputed in parallel
+ Vec3d n1 = e1.cross(e);
+ Vec3d n2 = e.cross(e3);
+ const double A1 = n1.norm();
+ const double A2 = n2.norm();
+ n1 /= A1;
+ n2 /= A2;
+
+ const double l = e.norm();
+ if (l < EPS)
+ {
+ 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;
+
+ double 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;
+ }
+
+ if (im1 > 0)
+ {
+ p1 += -im1*lambda*grad1;
+ }
+
+ if (im2 > 0)
+ {
+ p2 += -im2*lambda*grad2;
+ }
+
+ if (im3 > 0)
+ {
+ p3 += -im3*lambda*grad3;
+ }
+
+ return true;
+}
+
+} // imstk
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdDihedralConstraint.h b/Base/Constraint/imstkPbdDihedralConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..d44854ae0b2b5b0edbd0679a6af26f306f221c1c
--- /dev/null
+++ b/Base/Constraint/imstkPbdDihedralConstraint.h
@@ -0,0 +1,78 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef IMSTK_PBD_DIHEDRAL_CONSTRAINT_H
+#define IMSTK_PBD_DIHEDRAL_CONSTRAINT_H
+
+#include "imstkPbdConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \class DihedralConstraint
+///
+/// \brief Angular constraint between two triangular faces
+///
+class DihedralConstraint : public PbdConstraint
+{
+public:
+ ///
+ /// \brief Constructor
+ ///
+ DihedralConstraint() : PbdConstraint(4) {}
+
+ ///
+ /// \brief Returns PBD constraint of type Type::Dihedral
+ ///
+ Type getType() const { return Type::Dihedral; }
+
+ ///
+ /// \brief initConstraint
+ /// p3
+ /// / | \
+ /// / | \
+ /// p0 | p1
+ /// \ | /
+ /// \ | /
+ /// p2
+ /// \param model
+ /// \param pIdx1 index of p0
+ /// \param pIdx2 index of p1
+ /// \param pIdx3 index of p2
+ /// \param pIdx4 index of p3
+ /// \param k stiffness
+ ///
+ void initConstraint(PositionBasedModel& model, const unsigned int& pIdx1, const unsigned int& pIdx2,
+ const unsigned int& pIdx3, const unsigned int& pIdx4, const double k = 1e-3);
+
+ ///
+ /// \brief Solves the dihedral angular constraint
+ ///
+ bool solvePositionConstraint(PositionBasedModel &model);
+
+public:
+ double m_restAngle; ///> Rest angle
+ double m_stiffness; ///> Angular stiffness
+};
+}
+
+#endif // IMSTK_PBD_DIHEDRAL_CONSTRAINT_H
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdDistanceConstraint.cpp b/Base/Constraint/imstkPbdDistanceConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..358f4ee9c0607e0d00cb68af528d8928705c8c0e
--- /dev/null
+++ b/Base/Constraint/imstkPbdDistanceConstraint.cpp
@@ -0,0 +1,82 @@
+/*=========================================================================
+
+ 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 "imstkPbdDistanceConstraint.h"
+#include "imstkPbdModel.h"
+
+namespace imstk
+{
+
+void
+DistanceConstraint::initConstraint(PositionBasedModel &model, const unsigned int &pIdx1,
+ const unsigned int &pIdx2, const double k)
+{
+ m_bodies[0] = pIdx1;
+ m_bodies[1] = pIdx2;
+ m_stiffness = k;
+
+ auto state = model.getState();
+ const Vec3d &p1 = state->getInitialVertexPosition(pIdx1);
+ const Vec3d &p2 = state->getInitialVertexPosition(pIdx2);
+
+ m_restLength = (p1 - p2).norm();
+}
+
+bool
+DistanceConstraint::solvePositionConstraint(PositionBasedModel &model)
+{
+ const unsigned int i1 = m_bodies[0];
+ const unsigned int i2 = m_bodies[1];
+
+ auto state = model.getState();
+
+ Vec3d &p0 = state->getVertexPosition(i1);
+ Vec3d &p1 = state->getVertexPosition(i2);
+
+ const double im1 = state->getInvMass(i1);
+ const double im2 = state->getInvMass(i2);
+
+ const double wsum = im1 + im2;
+
+ if (wsum == 0.0)
+ {
+ return false;
+ }
+
+ Vec3d n = p1 - p0;
+ const double len = n.norm();
+ n /= len;
+
+ Vec3d gradC = m_stiffness*n*(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/Base/Constraint/imstkPbdDistanceConstraint.h b/Base/Constraint/imstkPbdDistanceConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..5df707cf455bf86ddaf1c7fa820ce529f965b880
--- /dev/null
+++ b/Base/Constraint/imstkPbdDistanceConstraint.h
@@ -0,0 +1,66 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef IMSTK_PBD_DISTANCE_CONSTRAINT_H
+#define IMSTK_PBD_DISTANCE_CONSTRAINT_H
+
+#include "imstkPbdConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \class DistanceConstraint
+///
+/// \brief Distance constraints between two nodal points
+///
+class DistanceConstraint : public PbdConstraint
+{
+public:
+ ///
+ /// \brief Constructor
+ ///
+ DistanceConstraint() : PbdConstraint(2) {}
+
+ ///
+ /// \brief Returns PBD constraint of type Type::Distance
+ ///
+ Type getType() const { return Type::Distance; }
+
+ ///
+ /// \brief Initializes the distance constraint
+ ///
+ void initConstraint(PositionBasedModel& model, const unsigned int& pIdx1,
+ const unsigned int& pIdx2, const double k = 1e-1);
+
+ ///
+ /// \brief Solves the Distance constraint
+ ///
+ bool solvePositionConstraint(PositionBasedModel &model);
+
+public:
+ double m_restLength; ///> Rest length between the nodes
+ double m_stiffness; ///> Stiffness of the constaint
+};
+
+}
+
+#endif // IMSTK_PBD_DISTANCE_CONSTRAINT_H
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdFEHexConstraint.h b/Base/Constraint/imstkPbdFEHexConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..6c8c22f7393e465775f4b1f36fe2dc9cb6298819
--- /dev/null
+++ b/Base/Constraint/imstkPbdFEHexConstraint.h
@@ -0,0 +1,70 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef IMSTK_PBD_FE_HEX_CONSTRAINT_H
+#define IMSTK_PBD_FE_HEX_CONSTRAINT_H
+
+#include "imstkPbdFEMConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \class FEMHexConstraint
+///
+/// \brief The FEMHexConstraint class class for constraint as the elastic energy
+/// computed by linear shape functions with hexahedral mesh.
+///
+class FEMHexConstraint : public FEMConstraint
+{
+public:
+ ///
+ /// \brief Constructor
+ ///
+ explicit FEMHexConstraint(MaterialType mtype = MaterialType::StVK) :
+ FEMConstraint(8, mtype) {}
+
+ ///
+ /// \brief Get the type of FEM constraint
+ ///
+ Type getType() const
+ {
+ return Type::FEMHex;
+ }
+
+ ///
+ /// \brief Initializes the FEM hexahedral element constraint
+ ///
+ bool initConstraint(PositionBasedModel& model, const unsigned int& pIdx1,
+ const unsigned int& pIdx2, const unsigned int& pIdx3,
+ const unsigned int& pIdx4, const unsigned int& pIdx5,
+ const unsigned int& pIdx6, const unsigned int& pIdx7,
+ const unsigned int& pIdx8);
+
+ ///
+ /// \brief Solves the FEM hexahedral element constraint
+ ///
+ bool solvePositionConstraint(PositionBasedModel &model);
+};
+
+}
+
+#endif // IMSTK_PBD_FE_HEX_CONSTRAINT_H
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdFEMConstraint.h b/Base/Constraint/imstkPbdFEMConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..ef6e7185ccd423cb5c48f1b65ec4de24ee783fe2
--- /dev/null
+++ b/Base/Constraint/imstkPbdFEMConstraint.h
@@ -0,0 +1,64 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef IMSTK_PBD_FEM_CONSTRAINT_H
+#define IMSTK_PBD_FEM_CONSTRAINT_H
+
+#include "imstkPbdConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \class FEMConstraint
+///
+/// \brief The FEMConstraint class for constraint as the elastic energy
+/// computed by linear shape functions with tetrahedral mesh.
+/// We provide several model for elastic energy including:
+/// Linear, Co-rotation, St Venant-Kirchhof and NeoHookean
+///
+class FEMConstraint : public PbdConstraint
+{
+public:
+ // Material type
+ enum class MaterialType
+ {
+ Linear,
+ Corotation,
+ StVK,
+ NeoHookean
+ };
+
+ ///
+ /// \brief
+ ///
+ explicit FEMConstraint(const unsigned int nP, MaterialType mtype = MaterialType::StVK) :
+ PbdConstraint(nP), m_material(mtype) {}
+
+public:
+ double m_Volume;
+ MaterialType m_material;
+ Mat3d m_invRestMat;
+};
+
+}
+
+#endif // IMSTK_PBD_FEM_CONSTRAINT_H
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdFETetConstraint.cpp b/Base/Constraint/imstkPbdFETetConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2ba201e7c404dab76d5cc6c874694823c1287fe7
--- /dev/null
+++ b/Base/Constraint/imstkPbdFETetConstraint.cpp
@@ -0,0 +1,223 @@
+/*=========================================================================
+
+ 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 "imstkPbdFETetConstraint.h"
+#include "imstkPbdModel.h"
+
+namespace imstk
+{
+
+bool
+FEMTetConstraint::initConstraint(PositionBasedModel &model,
+ const unsigned int &pIdx1, const unsigned int &pIdx2,
+ const unsigned int &pIdx3, const unsigned int &pIdx4)
+{
+ m_bodies[0] = pIdx1;
+ m_bodies[1] = pIdx2;
+ m_bodies[2] = pIdx3;
+ m_bodies[3] = pIdx4;
+
+ auto state = model.getState();
+
+ const Vec3d &p0 = state->getInitialVertexPosition(pIdx1);
+ const Vec3d &p1 = state->getInitialVertexPosition(pIdx2);
+ const Vec3d &p2 = state->getInitialVertexPosition(pIdx3);
+ const Vec3d &p3 = state->getInitialVertexPosition(pIdx4);
+
+ m_Volume = (1.0 / 6.0) * (p3 - p0).dot((p1 - p0).cross(p2 - p0));
+
+ Mat3d m;
+ m.col(0) = p0 - p3;
+ m.col(1) = p1 - p3;
+ m.col(2) = p2 - p3;
+
+ const double det = m.determinant();
+ if (fabs(det) > EPS)
+ {
+ m_invRestMat = m.inverse();
+ return true;
+ }
+
+ return false;
+}
+
+bool
+FEMTetConstraint::solvePositionConstraint(PositionBasedModel &model)
+{
+ const unsigned int i1 = m_bodies[0];
+ const unsigned int i2 = m_bodies[1];
+ const unsigned int i3 = m_bodies[2];
+ const unsigned int i4 = m_bodies[3];
+
+ auto state = model.getState();
+
+ Vec3d &p0 = state->getVertexPosition(i1);
+ Vec3d &p1 = state->getVertexPosition(i2);
+ Vec3d &p2 = state->getVertexPosition(i3);
+ Vec3d &p3 = state->getVertexPosition(i4);
+
+ //double currentVolume = (1.0 / 6.0) * (p3 - p0).dot((p1 - p0).cross(p2 - p0));
+
+ Mat3d m;
+ m.col(0) = p0 - p3;
+ m.col(1) = p1 - p3;
+ m.col(2) = p2 - p3;
+
+ // deformation gradient
+ const Mat3d F = m*m_invRestMat;
+ // First Piola-Kirchhoff tensor
+ Mat3d P;
+ // energy constraint
+ double C = 0;
+
+ const double mu = model.getFirstLame();
+ const double lambda = model.getSecondLame();
+
+ switch (m_material)
+ {
+
+ // P(F) = F*(2*mu*E + lambda*tr(E)*I)
+ // E = (F^T*F - I)/2
+ case MaterialType::StVK:
+ {
+ Mat3d E;
+ E(0, 0) = 0.5*(F(0, 0) * F(0, 0) + F(1, 0) * F(1, 0) + F(2, 0) * F(2, 0) - 1.0); // xx
+ E(1, 1) = 0.5*(F(0, 1) * F(0, 1) + F(1, 1) * F(1, 1) + F(2, 1) * F(2, 1) - 1.0); // yy
+ E(2, 2) = 0.5*(F(0, 2) * F(0, 2) + F(1, 2) * F(1, 2) + F(2, 2) * F(2, 2) - 1.0); // zz
+ E(0, 1) = 0.5*(F(0, 0) * F(0, 1) + F(1, 0) * F(1, 1) + F(2, 0) * F(2, 1)); // xy
+ E(0, 2) = 0.5*(F(0, 0) * F(0, 2) + F(1, 0) * F(1, 2) + F(2, 0) * F(2, 2)); // xz
+ E(1, 2) = 0.5*(F(0, 1) * F(0, 2) + F(1, 1) * F(1, 2) + F(2, 1) * F(2, 2)); // yz
+ E(1, 0) = E(0, 1);
+ E(2, 0) = E(0, 2);
+ E(2, 1) = E(1, 2);
+
+ P = 2 * mu*E;
+ double tr = E.trace();
+ double lt = lambda*tr;
+ P(0, 0) += lt;
+ P(1, 1) += lt;
+ P(2, 2) += lt;
+ P = F*P;
+
+ C = E(0, 0)*E(0, 0) + E(0, 1)*E(0, 1) + E(0, 2)*E(0, 2)
+ + E(1, 0)*E(1, 0) + E(1, 1)*E(1, 1) + E(1, 2)*E(1, 2)
+ + E(2, 0)*E(2, 0) + E(2, 1)*E(2, 1) + E(2, 2)*E(2, 2);
+ C = mu*C + 0.5*lambda*tr*tr;
+
+ break;
+ }
+
+ // P(F) = (2*mu*(F-R) + lambda*(J-1)*J*F^-T
+ case MaterialType::Corotation:
+ {
+ Eigen::JacobiSVD<Mat3d> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
+ Mat3d R = svd.matrixU()*svd.matrixV().adjoint();
+ Vec3d Sigma(svd.singularValues());
+ Mat3d invFT = svd.matrixU();
+ invFT.col(0) /= Sigma(0);
+ invFT.col(1) /= Sigma(1);
+ invFT.col(2) /= Sigma(2);
+ invFT *= svd.matrixV().adjoint();
+ double J = Sigma(0)*Sigma(1)*Sigma(2);
+ Mat3d FR = F - R;
+
+ P = 2 * mu*FR + lambda*(J - 1)*J*invFT;
+
+ C = FR(0, 0)*FR(0, 0) + FR(0, 1)*FR(0, 1) + FR(0, 2)*FR(0, 2)
+ + FR(1, 0)*FR(1, 0) + FR(1, 1)*FR(1, 1) + FR(1, 2)*FR(1, 2)
+ + FR(2, 0)*FR(2, 0) + FR(2, 1)*FR(2, 1) + FR(2, 2)*FR(2, 2);
+ C = mu*C + 0.5*lambda*(J - 1)*(J - 1);
+
+ break;
+ }
+ // P(F) = mu*(F - mu*F^-T) + lambda*log(J)F^-T;
+ case MaterialType::NeoHookean:
+ {
+ Mat3d invFT = F.inverse().transpose();
+ double logJ = log(F.determinant());
+ P = mu*(F - invFT) + lambda*logJ*invFT;
+
+ C = F(0, 0)*F(0, 0) + F(0, 1)*F(0, 1) + F(0, 2)*F(0, 2)
+ + F(1, 0)*F(1, 0) + F(1, 1)*F(1, 1) + F(1, 2)*F(1, 2)
+ + F(2, 0)*F(2, 0) + F(2, 1)*F(2, 1) + F(2, 2)*F(2, 2);
+
+ C = 0.5*mu*(C - 3) - mu*logJ + 0.5*lambda*logJ*logJ;
+
+ break;
+ }
+
+ case MaterialType::Linear:
+ {
+ break;
+ }
+
+ default:
+ {
+ LOG(WARNING) << "Material type not supported ! \n";
+ break;
+ }
+ }
+
+ const double im1 = state->getInvMass(i1);
+ const double im2 = state->getInvMass(i2);
+ const double im3 = state->getInvMass(i3);
+ const double im4 = state->getInvMass(i4);
+
+ Mat3d gradC = m_Volume*P*m_invRestMat.transpose();
+
+ 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 < EPS)
+ {
+ return false;
+ }
+
+ C = C*m_Volume;
+
+ 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));
+ }
+
+ return true;
+}
+
+} // imstk
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdFETetConstraint.h b/Base/Constraint/imstkPbdFETetConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..df3e0261b47377f03bf02774e18dfe3a134d3ea1
--- /dev/null
+++ b/Base/Constraint/imstkPbdFETetConstraint.h
@@ -0,0 +1,67 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef IMSTK_PBD_FE_TET_CONSTRAINT_H
+#define IMSTK_PBD_FE_TET_CONSTRAINT_H
+
+#include "imstkPbdFEMConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \class FEMTetConstraint
+///
+/// \brief The FEMTetConstraint class class for constraint as the elastic energy
+/// computed by linear shape functions with tetrahedral mesh.
+///
+class FEMTetConstraint : public FEMConstraint
+{
+public:
+ ///
+ /// \brief Constructor
+ ///
+ explicit FEMTetConstraint(MaterialType mtype = MaterialType::StVK) :
+ FEMConstraint(4, mtype) {}
+
+ ///
+ /// \brief Get the type of FEM constraint
+ ///
+ Type getType() const
+ {
+ return Type::FEMTet;
+ }
+
+ ///
+ /// \brief Initialize the tetrahedral FEM constraint
+ ///
+ bool initConstraint(PositionBasedModel& model, const unsigned int& pIdx1, const unsigned int& pIdx2,
+ const unsigned int& pIdx3, const unsigned int& pIdx4);
+
+ ///
+ /// \brief Solve the tetrahedral FEM constraint
+ ///
+ bool solvePositionConstraint(PositionBasedModel &model);
+};
+
+}
+
+#endif // IMSTK_PBD_FE_TET_CONSTRAINT_H
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdVolumeConstraint.cpp b/Base/Constraint/imstkPbdVolumeConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..206ef643234b4a129f8cd3fb6c1cfd219b143ea9
--- /dev/null
+++ b/Base/Constraint/imstkPbdVolumeConstraint.cpp
@@ -0,0 +1,109 @@
+/*=========================================================================
+
+ 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 "imstkPbdVolumeConstraint.h"
+#include "imstkPbdModel.h"
+
+namespace imstk
+{
+
+void
+VolumeConstraint::initConstraint(PositionBasedModel &model, const unsigned int &pIdx1,
+ const unsigned int &pIdx2, const unsigned int &pIdx3,
+ const unsigned int &pIdx4, const double k)
+{
+ m_bodies[0] = pIdx1;
+ m_bodies[1] = pIdx2;
+ m_bodies[2] = pIdx3;
+ m_bodies[3] = pIdx4;
+
+ m_stiffness = k;
+
+ auto state = model.getState();
+
+ const Vec3d &p0 = state->getInitialVertexPosition(pIdx1);
+ const Vec3d &p1 = state->getInitialVertexPosition(pIdx2);
+ const Vec3d &p2 = state->getInitialVertexPosition(pIdx3);
+ const Vec3d &p3 = state->getInitialVertexPosition(pIdx4);
+
+ m_restVolume = (1.0 / 6.0)*((p1 - p0).cross(p2 - p0)).dot(p3 - p0);
+}
+
+bool
+VolumeConstraint::solvePositionConstraint(PositionBasedModel &model)
+{
+ const unsigned int i1 = m_bodies[0];
+ const unsigned int i2 = m_bodies[1];
+ const unsigned int i3 = m_bodies[2];
+ const unsigned int i4 = m_bodies[3];
+
+ auto state = model.getState();
+
+ Vec3d &x1 = state->getVertexPosition(i1);
+ Vec3d &x2 = state->getVertexPosition(i2);
+ Vec3d &x3 = state->getVertexPosition(i3);
+ Vec3d &x4 = state->getVertexPosition(i4);
+
+ const double im1 = state->getInvMass(i1);
+ const double im2 = state->getInvMass(i2);
+ const double im3 = state->getInvMass(i3);
+ const double im4 = state->getInvMass(i4);
+
+ 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 double 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 (im1 > 0)
+ {
+ x2 += -im2*lambda*grad2;
+ }
+
+ if (im3 > 0)
+ {
+ x3 += -im3*lambda*grad3;
+ }
+
+ if (im4 > 0)
+ {
+ x4 += -im4*lambda*grad4;
+ }
+
+ return true;
+}
+
+} // imstk
\ No newline at end of file
diff --git a/Base/Constraint/imstkPbdVolumeConstraint.h b/Base/Constraint/imstkPbdVolumeConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..efb1d731ab6ad11eb05f11c857ef9cc90a0c297f
--- /dev/null
+++ b/Base/Constraint/imstkPbdVolumeConstraint.h
@@ -0,0 +1,69 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef IMSTK_PBD_VOLUME_CONSTRAINT_H
+#define IMSTK_PBD_VOLUME_CONSTRAINT_H
+
+#include "imstkPbdConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \class VolumeConstraint
+///
+/// \brief Volume constraint for tetrahedral element
+///
+class VolumeConstraint : public PbdConstraint
+{
+public:
+ ///
+ /// \brief constructor
+ ///
+ VolumeConstraint() : PbdConstraint(4) {}
+
+ ///
+ /// \brief Returns PBD constraint of type Type::Volume
+ ///
+ Type getType() const
+ {
+ return Type::Volume;
+ }
+
+ ///
+ /// \brief Initializes the volume constraint
+ ///
+ void initConstraint(PositionBasedModel& model, const unsigned int& pIdx1, const unsigned int& pIdx2,
+ const unsigned int& pIdx3, const unsigned int& pIdx4, const double k = 2.0);
+
+ ///
+ /// \brief Solves the volume constraint
+ ///
+ bool solvePositionConstraint(PositionBasedModel &model);
+
+public:
+ double m_restVolume; ///> Rest volume
+ double m_stiffness; ///> Stiffness of the volume constraint
+};
+
+}
+
+#endif // IMSTK_PBD_DISTANCE_CONSTRAINT_H
\ No newline at end of file
diff --git a/Base/SceneElements/Objects/imstkPbdModel.cpp b/Base/SceneElements/Objects/imstkPbdModel.cpp
index 8b72205f097780ec3e6c585f3fbe159b24db0951..2e250a1bbf7060e0372dd58372b4420ff66dd0e3 100644
--- a/Base/SceneElements/Objects/imstkPbdModel.cpp
+++ b/Base/SceneElements/Objects/imstkPbdModel.cpp
@@ -1,6 +1,12 @@
#include "imstkPbdModel.h"
#include "imstkTetrahedralMesh.h"
#include "imstkSurfaceMesh.h"
+#include "imstkPbdVolumeConstraint.h"
+#include "imstkPbdDistanceConstraint.h"
+#include "imstkPbdDihedralConstraint.h"
+#include "imstkPbdAreaConstraint.h"
+#include "imstkPbdFETetConstraint.h"
+#include "imstkPbdFEHexConstraint.h"
#include <g3log/g3log.hpp>
diff --git a/Base/SceneElements/Objects/imstkPbdModel.h b/Base/SceneElements/Objects/imstkPbdModel.h
index 5226a792f98238dce2c3910b17cad8c5519f8fd7..5356e6acc804d965797357e84d8cfade26e72c71 100644
--- a/Base/SceneElements/Objects/imstkPbdModel.h
+++ b/Base/SceneElements/Objects/imstkPbdModel.h
@@ -5,6 +5,7 @@
#include <Eigen/Dense>
#include "imstkPbdConstraint.h"
+#include "imstkPbdFEMConstraint.h"
#include "imstkPbdState.h"
namespace imstk
diff --git a/Examples/Sandbox/main.cpp b/Examples/Sandbox/main.cpp
index 79b5700e75affcb9db834bd5013b14425f981a7f..2139a8b11a455bc686ac26e13d814281909401a9 100644
--- a/Examples/Sandbox/main.cpp
+++ b/Examples/Sandbox/main.cpp
@@ -104,7 +104,7 @@ int main()
//testMultiTextures();
//testMeshCCD();
- testPenaltyRigidCollision();
+ //testPenaltyRigidCollision();
//testTwoFalcons();
//testObjectController();
//testCameraController();
@@ -123,7 +123,7 @@ int main()
//testTwoOmnis();
//testVectorPlotters();
//testPbdVolume();
- //testPbdCloth();
+ testPbdCloth();
// int n;
// std::cout << "testPbdCollision(): 1" << std::endl;
@@ -1266,7 +1266,7 @@ void testPbdCloth()
/*Constraint configuration*/"Dihedral 0.001",
/*Mass*/1.0,
/*Gravity*/"0 -9.8 0",
- /*TimeStep*/0.001,
+ /*TimeStep*/0.01,
/*FixedPoint*/"1 20",
/*NumberOfIterationInConstraintSolver*/5
);
@@ -1321,7 +1321,7 @@ void testPbdCollision()
deformableObj->setCollidingGeometry(surfMesh);
deformableObj->setPhysicsGeometry(volTetMesh);
deformableObj->setPhysicsToCollidingMap(deformMapP2C);
- deformableObj->setPhysicsToVisualMap(deformMapP2V);
+ deformableObj->setPhysicsToVisualMap(deformMapP2V);
deformableObj->setCollidingToVisualMap(deformMapC2V);
deformableObj->init(/*Number of Constraints*/1,
/*Constraint configuration*/"FEM NeoHookean 1.0 0.3",
@@ -1446,7 +1446,7 @@ void testPbdCollision()
volTetMesh1->extractSurfaceMesh(surfMesh1);
volTetMesh1->extractSurfaceMesh(surfMeshVisual1);
-
+
auto deformMapP2V1 = std::make_shared<imstk::OneToOneMap>();
deformMapP2V1->setMaster(volTetMesh1);
@@ -1468,7 +1468,7 @@ void testPbdCollision()
deformableObj1->setCollidingGeometry(surfMesh1);
deformableObj1->setPhysicsGeometry(volTetMesh1);
deformableObj1->setPhysicsToCollidingMap(deformMapP2C1);
- deformableObj1->setPhysicsToVisualMap(deformMapP2V1);
+ deformableObj1->setPhysicsToVisualMap(deformMapP2V1);
deformableObj1->setCollidingToVisualMap(deformMapC2V1);
deformableObj1->init(/*Number of Constraints*/1,
/*Constraint configuration*/"FEM NeoHookean 10.0 0.5",
@@ -1491,7 +1491,7 @@ void testPbdCollision()
}
else{
// floor
-
+
std::vector<imstk::Vec3d> vertList;
double width = 100.0;
double height = 100.0;
@@ -1717,7 +1717,7 @@ void testLineMesh()
scene->addSceneObject(blade);
}
-
+
if (clothTest){