diff --git a/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.cpp b/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8319fe50f98ed4a767073df31b8e98190ea50fe8
--- /dev/null
+++ b/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.cpp
@@ -0,0 +1,60 @@
+/*=========================================================================
+
+ 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 "imstkLinearProjectionConstraint.h"
+
+namespace imstk
+{
+
+LinearProjectionConstraint::
+LinearProjectionConstraint(const unsigned int& nodeId, const bool isFixed /*= false*/)
+{
+ m_nodeId = nodeId;
+ if (isFixed)
+ {
+ m_projection = Mat3d::Zero();
+ m_isFixedConstraint = true;
+ }
+}
+
+void
+LinearProjectionConstraint::setProjection(const unsigned int& nodeId, const Vec3d& p, const Vec3d& q /*= Vec3d::Zero()*/)
+{
+ m_nodeId = nodeId;
+ m_projection = Mat3d::Identity() - p*p.transpose() - q*q.transpose();
+}
+
+void
+LinearProjectionConstraint::setProjectorToDirichlet(const unsigned int& nodeId)
+{
+ m_nodeId = nodeId;
+ m_projection = Mat3d::Zero();
+ m_isFixedConstraint = true;
+}
+
+void
+LinearProjectionConstraint::reset()
+{
+ m_projection = Mat3d::Identity();
+ m_value = Vec3d(0., 0., 0.);
+}
+
+} // imstk
\ No newline at end of file
diff --git a/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.h b/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..da4bba63d91a2435b947be5e740911c202b6db98
--- /dev/null
+++ b/Base/Constraint/PbdConstraints/imstkLinearProjectionConstraint.h
@@ -0,0 +1,105 @@
+/*=========================================================================
+
+ 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 imstkLinearProjectionConstraint_h
+#define imstkLinearProjectionConstraint_h
+
+#include "imstkMath.h"
+
+namespace imstk
+{
+
+///
+/// \class LinearProjectionConstraint
+///
+/// \brief Linear projection constraint
+///
+class LinearProjectionConstraint
+{
+public:
+ ///
+ /// \brief Constructor
+ ///
+ LinearProjectionConstraint(const unsigned int& nodeId, const bool isFixed = false);
+ LinearProjectionConstraint() = delete;
+
+ ///
+ /// \brief Destructor
+ ///
+ ~LinearProjectionConstraint() = default;
+
+ ///
+ /// \brief Form the projection
+ ///
+ void setProjection(const unsigned int& nodeId, const Vec3d& p, const Vec3d& q = Vec3d::Zero());
+
+ ///
+ /// \brief Set the projector to simulate Dirichlet conditions
+ ///
+ void setProjectorToDirichlet(const unsigned int& nodeId);
+
+ ///
+ /// \brief Reset the linear projector
+ ///
+ void reset();
+
+ ///
+ /// \brief Set the value in the restricted subspace
+ ///
+ void setValue(const Vec3d& v)
+ {
+ m_value = v;
+ }
+
+ ///
+ /// \brief Get the projector
+ ///
+ inline const Mat3d& getProjector() const
+ {
+ return m_projection;
+ }
+
+ ///
+ /// \brief Get the value
+ ///
+ inline const Vec3d& getValue() const
+ {
+ return m_value;
+ }
+
+ ///
+ /// \brief Get the node id
+ ///
+ inline const unsigned int& getNodeId() const
+ {
+ return m_nodeId;
+ }
+
+private:
+ unsigned int m_nodeId; ///> Node id
+ bool m_isFixedConstraint = false; ///> Flag to know if that node is fixed
+ Mat3d m_projection = Mat3d::Identity(); ///> Orthogonal projector
+ Vec3d m_value = Vec3d(0., 0., 0.); ///> Value in the subspace: range(I-m_projector)
+};
+
+} // imstk
+
+#endif // imstkLinearProjectionConstraint_h
\ No newline at end of file
diff --git a/Base/Solvers/CMakeLists.txt b/Base/Solvers/CMakeLists.txt
index 996abd97b496e7d9f7bc0ae59f93fea8ec307cf0..f16a99e2d1106f3a72042d58de85c3e7413bdb5e 100644
--- a/Base/Solvers/CMakeLists.txt
+++ b/Base/Solvers/CMakeLists.txt
@@ -5,6 +5,7 @@ include(imstkAddLibrary)
imstk_add_library( Solvers
DEPENDS
Core
+ Constraints
)
#-----------------------------------------------------------------------------
diff --git a/Base/Solvers/imstkConjugateGradient.cpp b/Base/Solvers/imstkConjugateGradient.cpp
index 85d6d0bdaa77511b68020025eb36b58c552d65f3..7f96b5c176cb3eb9fb153b9f24b74b7e6db1bb18 100644
--- a/Base/Solvers/imstkConjugateGradient.cpp
+++ b/Base/Solvers/imstkConjugateGradient.cpp
@@ -34,13 +34,26 @@ ConjugateGradient::ConjugateGradient(const SparseMatrixd& A, const Vectord& rhs)
}
void
-ConjugateGradient::filter(Vectord& x, const std::vector<size_t>& filter)
+ConjugateGradient::applyLinearProjectionFilter(Vectord& x, const std::vector<LinearProjectionConstraint>& linProj, const bool setVal)
{
- size_t threeI;
- for (auto &i: filter)
+ Vec3d p;
+ for (auto &localProjector : linProj)
{
- threeI = 3 * i;
- x(threeI) = x(threeI + 1) = x(threeI + 2) = 0.0;
+ const auto threeI = 3 * localProjector.getNodeId();
+ p = Vec3d(x(threeI), x(threeI + 1), x(threeI + 2));
+
+ if (!setVal)
+ {
+ p = localProjector.getProjector()*p;
+ }
+ else
+ {
+ p = (Mat3d::Identity() - localProjector.getProjector())*localProjector.getValue();
+ }
+
+ x(threeI) = p.x();
+ x(threeI + 1) = p.y();
+ x(threeI + 2) = p.z();
}
}
@@ -53,7 +66,7 @@ ConjugateGradient::solve(Vectord& x)
return;
}
- if (m_linearSystem->getFilter().size() == 0)
+ if (m_linearSystem->getLinearProjectors().size() == 0)
{
x = m_cgSolver.solve(m_linearSystem->getRHSVector());
}
@@ -66,15 +79,16 @@ ConjugateGradient::solve(Vectord& x)
void
ConjugateGradient::modifiedCGSolve(Vectord& x)
{
- const auto &fixedNodes = m_linearSystem->getFilter();
+ const auto &linearProjectors = m_linearSystem->getLinearProjectors();
const auto &b = m_linearSystem->getRHSVector();
const auto &A = m_linearSystem->getMatrix();
// Set the initial guess to zero
x.setZero();
+ applyLinearProjectionFilter(x, linearProjectors, true);
auto res = b;
- filter(res, fixedNodes);
+ applyLinearProjectionFilter(res, linearProjectors, false);
auto c = res;
auto delta = res.dot(res);
auto deltaPrev = delta;
@@ -87,7 +101,7 @@ ConjugateGradient::modifiedCGSolve(Vectord& x)
while (delta > eps)
{
q = A * c;
- filter(q, fixedNodes);
+ applyLinearProjectionFilter(q, linearProjectors, false);
dotval = c.dot(q);
if (dotval != 0.0)
{
@@ -104,7 +118,7 @@ ConjugateGradient::modifiedCGSolve(Vectord& x)
delta = res.dot(res);
c *= delta / deltaPrev;
c += res;
- filter(c, fixedNodes);
+ applyLinearProjectionFilter(c, linearProjectors, false);
if (++iterNum >= m_maxIterations)
{
diff --git a/Base/Solvers/imstkConjugateGradient.h b/Base/Solvers/imstkConjugateGradient.h
index 95cf588da3ccf262ba1534e46e09bfcabc1cc026..bce5bf4b4216832d7d1e217be18ece3c5a391215 100644
--- a/Base/Solvers/imstkConjugateGradient.h
+++ b/Base/Solvers/imstkConjugateGradient.h
@@ -95,8 +95,7 @@ public:
///
/// \brief Apply a filter to the vector supplied
///
- void filter(Vectord& x, const std::vector<size_t>& filter);
-
+ void applyLinearProjectionFilter(Vectord& x, const std::vector<LinearProjectionConstraint>& linProj, const bool setVal);
private:
///
diff --git a/Base/Solvers/imstkNewtonSolver.cpp b/Base/Solvers/imstkNewtonSolver.cpp
index 0fd821795e87bdc5c9182fdee830db6caf97dc8d..9adddbf6085efaeae99ef68918729ab57fbf97be 100644
--- a/Base/Solvers/imstkNewtonSolver.cpp
+++ b/Base/Solvers/imstkNewtonSolver.cpp
@@ -144,7 +144,7 @@ NewtonSolver::updateJacobian(const Vectord& x)
auto &b = m_nonLinearSystem->m_F(x, m_isSemiImplicit);
auto linearSystem = std::make_shared<LinearSolverType::LinearSystemType>(A, b);
- linearSystem->setFilter(m_nonLinearSystem->getFilter());
+ linearSystem->setLinearProjectors(m_nonLinearSystem->getLinearProjectors());
m_linearSolver->setSystem(linearSystem);
return b.dot(b);
diff --git a/Base/Solvers/imstkNonlinearSystem.h b/Base/Solvers/imstkNonlinearSystem.h
index e4d725c018e83a8385c17b75fdc1e8736346c2a5..d2e3bebcdf54b6ff297b4e3c68c6fd3ae2e55520 100644
--- a/Base/Solvers/imstkNonlinearSystem.h
+++ b/Base/Solvers/imstkNonlinearSystem.h
@@ -24,6 +24,7 @@
#include <stdio.h>
#include "imstkMath.h"
+#include "imstkLinearProjectionConstraint.h"
namespace imstk
{
@@ -89,16 +90,16 @@ namespace imstk
/// \brief Get the vector denoting the filter
///
- void setFilter(std::vector<size_t> & f)
+ void setLinearProjectors(std::vector<LinearProjectionConstraint>& f)
{
- m_Filter = &f;
+ m_LinearProjConstraints = &f;
}
/// \brief Get the vector denoting the filter
///
- std::vector<size_t>& getFilter()
+ std::vector<LinearProjectionConstraint>& getLinearProjectors()
{
- return *m_Filter;
+ return *m_LinearProjConstraints;
}
///
@@ -124,7 +125,7 @@ public:
UpdateFunctionType m_FUpdate;
UpdatePrevStateFunctionType m_FUpdatePrevState;
- std::vector<size_t> *m_Filter;
+ std::vector<LinearProjectionConstraint> *m_LinearProjConstraints;
};
} // imstk
diff --git a/Examples/Sandbox/main.cpp b/Examples/Sandbox/main.cpp
index 47f556b81f68eb1a8503c84e92fb03f72b6c0024..f384ac5da52d1c70fa479f88f223aee6aa44a1aa 100644
--- a/Examples/Sandbox/main.cpp
+++ b/Examples/Sandbox/main.cpp
@@ -139,7 +139,7 @@ int main()
//testPbdVolume();
//testPbdCloth();
//testPbdCollision();
- //testDeformableBody();
+ testDeformableBody();
/*------------------
@@ -155,7 +155,7 @@ int main()
------------------*/
//testObjectController();
//testTwoFalcons();
- testCameraController();
+ //testCameraController();
//testTwoOmnis();
//testLapToolController();
@@ -1293,7 +1293,17 @@ void testDeformableBody()
auto nlSystem = std::make_shared<NonLinearSystem>(
dynaModel->getFunction(),
dynaModel->getFunctionGradient());
- nlSystem->setFilter(dynaModel->getFixNodeIds());
+
+ std::vector<LinearProjectionConstraint> projList;
+ for (auto i : dynaModel->getFixNodeIds())
+ {
+ auto s = LinearProjectionConstraint(i, false);
+ s.setProjectorToDirichlet(i);
+ s.setValue(Vec3d(0.001, 0, 0));
+ projList.push_back(s);
+ }
+ nlSystem->setLinearProjectors(projList);
+
nlSystem->setUnknownVector(dynaModel->getUnknownVec());
nlSystem->setUpdateFunction(dynaModel->getUpdateFunction());
nlSystem->setUpdatePreviousStatesFunction(dynaModel->getUpdatePrevStateFunction());