From 26ef0e093ad46c0065c5eaccb74bc7db34ebcfd2 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 15 Apr 2020 13:32:16 -0700 Subject: [PATCH] add full preconditioner for KKT system --- src/BulletSoftBody/btConjugateResidual.h | 37 +++++--- .../btDeformableBackwardEulerObjective.h | 1 - src/BulletSoftBody/btDeformableBodySolver.cpp | 5 +- .../btDeformableContactConstraint.cpp | 2 +- .../btDeformableMultiBodyConstraintSolver.cpp | 1 - src/BulletSoftBody/btPreconditioner.h | 95 ++++++++++++++++++- 6 files changed, 119 insertions(+), 22 deletions(-) diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h index 76c3f42cb..0f2b79713 100644 --- a/src/BulletSoftBody/btConjugateResidual.h +++ b/src/BulletSoftBody/btConjugateResidual.h @@ -26,21 +26,21 @@ template class btConjugateResidual { typedef btAlignedObjectArray TVStack; - TVStack r,p,z,temp_p, temp_r; + TVStack r,p,z,temp_p, temp_r, best_x; // temp_r = A*r // temp_p = A*p // z = M^(-1) * temp_p = M^(-1) * A * p int max_iterations; - btScalar tolerance_squared; + btScalar tolerance_squared, best_r; int count; int total_it; public: btConjugateResidual(const int max_it_in) - : max_iterations(1000) + : max_iterations(max_it_in) { count = 0; total_it = 0; - tolerance_squared = 1e-3; + tolerance_squared = 1e-2; } virtual ~btConjugateResidual(){} @@ -88,18 +88,24 @@ public: multAndAddTo(alpha, p, x); // r -= alpha * z; multAndAddTo(-alpha, z, r); - if (norm(r) < tolerance_squared) { - if (verbose) - { - std::cout << "ConjugateResidual iterations " << k << std::endl; - } - return k; - } - else + btScalar norm_r = norm(r); + if (norm_r < best_r) { - if (verbose) + best_x = x; + best_r = norm_r; + if (norm_r < tolerance_squared) { + if (verbose) + { + std::cout << "ConjugateResidual iterations " << k << std::endl; + } + return k; + } + else { - std::cout << "ConjugateResidual iterations " << k << " has residual "<< norm(r) << std::endl; + if (verbose) + { + std::cout << "ConjugateResidual iterations " << k << " has residual "<< norm_r << std::endl; + } } } // temp_r = A * r; @@ -116,6 +122,7 @@ public: { std::cout << "ConjugateResidual max iterations reached " << max_iterations << std::endl; } + x = best_x; return max_iterations; } @@ -126,6 +133,8 @@ public: z.resize(b.size()); temp_p.resize(b.size()); temp_r.resize(b.size()); + best_x.resize(b.size()); + best_r = SIMD_INFINITY; } TVStack sub(const TVStack& a, const TVStack& b) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index d12ea9104..dcabc5ef4 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -161,7 +161,6 @@ public: { for (int n = 0; n < lm.m_num_nodes; ++n) { - btScalar diff = lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]); extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]); } } diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index bcebe2144..d3899289b 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -18,7 +18,7 @@ #include "btDeformableBodySolver.h" #include "btSoftBodyInternals.h" #include "LinearMath/btQuickprof.h" -static const int kMaxConjugateGradientIterations = 5; +static const int kMaxConjugateGradientIterations = 50; btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) , m_cg(kMaxConjugateGradientIterations) @@ -52,7 +52,6 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { m_dv[i] = x[i]; } -// m_objective->m_projection.enforceConstraints(x); updateVelocity(); } else @@ -211,7 +210,7 @@ void btDeformableBodySolver::updateDv(btScalar scale) void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { - m_cr.solve(*m_objective, ddv, residual, true); + m_cr.solve(*m_objective, ddv, residual, false); } void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies, btScalar dt) diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 0cb817318..046a770cf 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -212,7 +212,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btVector3 va = getVa(); btVector3 vb = getVb(); btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt btScalar residualSquare = dn*dn; btVector3 impulse = m_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ; diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 8b0b5bc7a..c8cc47923 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -40,7 +40,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) { -#define VERBOSE_RESIDUAL_PRINTF 1 #ifdef VERBOSE_RESIDUAL_PRINTF printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); #endif diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h index 278caaee9..2799e13e0 100644 --- a/src/BulletSoftBody/btPreconditioner.h +++ b/src/BulletSoftBody/btPreconditioner.h @@ -121,7 +121,7 @@ public: } } m_inv_S.resize(m_projections.m_lagrangeMultipliers.size()); - printf("S.size() = %d \n", m_inv_S.size()); +// printf("S.size() = %d \n", m_inv_S.size()); buildDiagonalS(m_inv_A, m_inv_S); for (int i = 0; i < m_inv_S.size(); ++i) { @@ -178,7 +178,8 @@ public: } } } - +#define USE_FULL_PRECONDITIONER +#ifndef USE_FULL_PRECONDITIONER virtual void operator()(const TVStack& x, TVStack& b) { btAssert(b.size() == x.size()); @@ -192,6 +193,96 @@ public: b[i+offset] = x[i+offset] * m_inv_S[i]; } } +#else + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + int offset = m_inv_A.size(); + + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = x[i] * m_inv_A[i]; + } + + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i+offset].setZero(); + } + + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + // C * x + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + b[offset+c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]); + } + } + } + + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i+offset] = b[i+offset] * m_inv_S[i]; + } + + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i].setZero(); + } + + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + b[lm.m_indices[i]] += b[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + } + + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = (x[i] - b[i]) * m_inv_A[i]; + } + + TVStack t; + t.resize(b.size()); + for (int i = 0; i < m_inv_S.size(); ++i) + { + t[i+offset] = x[i+offset] * m_inv_S[i]; + } + for (int i = 0; i < m_inv_A.size(); ++i) + { + t[i].setZero(); + } + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + t[lm.m_indices[i]] += t[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] += t[i] * m_inv_A[i]; + } + + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i+offset] -= x[i+offset] * m_inv_S[i]; + } + } +#endif }; #endif /* BT_PRECONDITIONER_H */