From 5fe898f9f2ff79f937c1e1a9293fa0c4e3314ad9 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 27 Apr 2020 15:48:51 -0700 Subject: [PATCH] refactor krylov solver --- src/BulletSoftBody/btConjugateGradient.h | 85 +++----------- src/BulletSoftBody/btConjugateResidual.h | 103 ++++------------- .../btDeformableContactConstraint.cpp | 1 - src/BulletSoftBody/btKrylovSolver.h | 108 ++++++++++++++++++ 4 files changed, 145 insertions(+), 152 deletions(-) create mode 100644 src/BulletSoftBody/btKrylovSolver.h diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index bd51e584b..f56dbc188 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -15,24 +15,17 @@ #ifndef BT_CONJUGATE_GRADIENT_H #define BT_CONJUGATE_GRADIENT_H -#include -#include -#include -#include -#include -#include "LinearMath/btQuickprof.h" +#include "btKrylovSolver.h" template -class btConjugateGradient +class btConjugateGradient : public btKrylovSolver { typedef btAlignedObjectArray TVStack; + typedef btKrylovSolver Base; TVStack r,p,z,temp; - int max_iterations; - btScalar tolerance_squared; public: btConjugateGradient(const int max_it_in) - : max_iterations(max_it_in) + : btKrylovSolver(max_it_in, SIMD_EPSILON) { - tolerance_squared = 1e-5; } virtual ~btConjugateGradient(){} @@ -45,13 +38,13 @@ public: reinitialize(b); // r = b - A * x --with assigned dof zeroed out A.multiply(x, temp); - r = sub(b, temp); + r = this->sub(b, temp); A.project(r); // z = M^(-1) * r A.precondition(r, z); A.project(z); - btScalar r_dot_z = dot(z,r); - if (r_dot_z <= tolerance_squared) { + btScalar r_dot_z = this->dot(z,r); + if (r_dot_z <= Base::m_tolerance) { if (verbose) { std::cout << "Iteration = 0" << std::endl; @@ -61,11 +54,11 @@ public: } p = z; btScalar r_dot_z_new = r_dot_z; - for (int k = 1; k <= max_iterations; k++) { + for (int k = 1; k <= Base::m_maxIterations; k++) { // temp = A*p A.multiply(p, temp); A.project(temp); - if (dot(p,temp) < SIMD_EPSILON) + if (this->dot(p,temp) < SIMD_EPSILON) { if (verbose) std::cout << "Encountered negative direction in CG!" << std::endl; @@ -76,16 +69,16 @@ public: return k; } // alpha = r^T * z / (p^T * A * p) - btScalar alpha = r_dot_z_new / dot(p, temp); + btScalar alpha = r_dot_z_new / this->dot(p, temp); // x += alpha * p; - multAndAddTo(alpha, p, x); + this->multAndAddTo(alpha, p, x); // r -= alpha * temp; - multAndAddTo(-alpha, temp, r); + this->multAndAddTo(-alpha, temp, r); // z = M^(-1) * r A.precondition(r, z); r_dot_z = r_dot_z_new; - r_dot_z_new = dot(r,z); - if (r_dot_z_new < tolerance_squared) { + r_dot_z_new = this->dot(r,z); + if (r_dot_z_new < Base::m_tolerance) { if (verbose) { std::cout << "ConjugateGradient iterations " << k << std::endl; @@ -94,13 +87,13 @@ public: } btScalar beta = r_dot_z_new/r_dot_z; - p = multAndAdd(beta, p, z); + p = this->multAndAdd(beta, p, z); } if (verbose) { - std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl; + std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << std::endl; } - return max_iterations; + return Base::m_maxIterations; } void reinitialize(const TVStack& b) @@ -110,49 +103,5 @@ public: z.resize(b.size()); temp.resize(b.size()); } - - TVStack sub(const TVStack& a, const TVStack& b) - { - // c = a-b - btAssert(a.size() == b.size()); - TVStack c; - c.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - { - c[i] = a[i] - b[i]; - } - return c; - } - - btScalar squaredNorm(const TVStack& a) - { - return dot(a,a); - } - - btScalar dot(const TVStack& a, const TVStack& b) - { - btScalar ans(0); - for (int i = 0; i < a.size(); ++i) - ans += a[i].dot(b[i]); - return ans; - } - - void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) - { -// result += s*a - btAssert(a.size() == result.size()); - for (int i = 0; i < a.size(); ++i) - result[i] += s * a[i]; - } - - TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) - { - // result = a*s + b - TVStack result; - result.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - result[i] = s * a[i] + b[i]; - return result; - } }; #endif /* btConjugateGradient_h */ diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h index cb2a48762..d4739da83 100644 --- a/src/BulletSoftBody/btConjugateResidual.h +++ b/src/BulletSoftBody/btConjugateResidual.h @@ -15,28 +15,22 @@ #ifndef BT_CONJUGATE_RESIDUAL_H #define BT_CONJUGATE_RESIDUAL_H -#include -#include -#include -#include -#include -#include -#include "LinearMath/btQuickprof.h" +#include "btKrylovSolver.h" + template -class btConjugateResidual +class btConjugateResidual : public btKrylovSolver { typedef btAlignedObjectArray TVStack; + typedef btKrylovSolver Base; 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, best_r; + btScalar best_r; public: btConjugateResidual(const int max_it_in) - : max_iterations(max_it_in) + : Base(max_it_in, 1e-8) { - tolerance_squared = 1e-5; } virtual ~btConjugateResidual(){} @@ -49,12 +43,12 @@ public: reinitialize(b); // r = b - A * x --with assigned dof zeroed out A.multiply(x, temp_r); // borrow temp_r here to store A*x - r = sub(b, temp_r); + r = this->sub(b, temp_r); // z = M^(-1) * r A.precondition(r, z); // borrow z to store preconditioned r r = z; - btScalar residual_norm = norm(r); - if (residual_norm <= tolerance_squared) { + btScalar residual_norm = this->norm(r); + if (residual_norm <= Base::m_tolerance) { return 0; } p = r; @@ -63,41 +57,41 @@ public: A.multiply(p, temp_p); // temp_r = A*r temp_r = temp_p; - r_dot_Ar = dot(r, temp_r); - for (int k = 1; k <= max_iterations; k++) { + r_dot_Ar = this->dot(r, temp_r); + for (int k = 1; k <= Base::m_maxIterations; k++) { // z = M^(-1) * Ap A.precondition(temp_p, z); // alpha = r^T * A * r / (Ap)^T * M^-1 * Ap) - btScalar alpha = r_dot_Ar / dot(temp_p, z); + btScalar alpha = r_dot_Ar / this->dot(temp_p, z); // x += alpha * p; - multAndAddTo(alpha, p, x); + this->multAndAddTo(alpha, p, x); // r -= alpha * z; - multAndAddTo(-alpha, z, r); - btScalar norm_r = norm(r); + this->multAndAddTo(-alpha, z, r); + btScalar norm_r = this->norm(r); if (norm_r < best_r) { best_x = x; best_r = norm_r; - if (norm_r < tolerance_squared) { + if (norm_r < Base::m_tolerance) { return k; } } // temp_r = A * r; A.multiply(r, temp_r); - r_dot_Ar_new = dot(r, temp_r); + r_dot_Ar_new = this->dot(r, temp_r); btScalar beta = r_dot_Ar_new/r_dot_Ar; r_dot_Ar = r_dot_Ar_new; // p = beta*p + r; - p = multAndAdd(beta, p, r); + p = this->multAndAdd(beta, p, r); // temp_p = beta*temp_p + temp_r; - temp_p = multAndAdd(beta, temp_p, temp_r); + temp_p = this->multAndAdd(beta, temp_p, temp_r); } if (verbose) { std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl; } x = best_x; - return max_iterations; + return Base::m_maxIterations; } void reinitialize(const TVStack& b) @@ -110,63 +104,6 @@ public: best_x.resize(b.size()); best_r = SIMD_INFINITY; } - - TVStack sub(const TVStack& a, const TVStack& b) - { - // c = a-b - btAssert(a.size() == b.size()); - TVStack c; - c.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - { - c[i] = a[i] - b[i]; - } - return c; - } - - btScalar squaredNorm(const TVStack& a) - { - return dot(a,a); - } - - btScalar norm(const TVStack& a) - { - btScalar ret = 0; - for (int i = 0; i < a.size(); ++i) - { - for (int d = 0; d < 3; ++d) - { - ret = btMax(ret, btFabs(a[i][d])); - } - } - return ret; - } - - btScalar dot(const TVStack& a, const TVStack& b) - { - btScalar ans(0); - for (int i = 0; i < a.size(); ++i) - ans += a[i].dot(b[i]); - return ans; - } - - void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) - { - // result += s*a - btAssert(a.size() == result.size()); - for (int i = 0; i < a.size(); ++i) - result[i] += s * a[i]; - } - - TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) - { - // result = a*s + b - TVStack result; - result.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - result[i] = s * a[i] + b[i]; - return result; - } }; #endif /* btConjugateResidual_h */ diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 4956c0cf9..dda9d5ef6 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -247,7 +247,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv { // separating in the normal direction m_static = false; -// m_total_tangent_dv = btVector3(0,0,0); impulse_tangent.setZero(); } else diff --git a/src/BulletSoftBody/btKrylovSolver.h b/src/BulletSoftBody/btKrylovSolver.h new file mode 100644 index 000000000..61954bd60 --- /dev/null +++ b/src/BulletSoftBody/btKrylovSolver.h @@ -0,0 +1,108 @@ +/* + Written by Xuchen Han + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_KRYLOV_SOLVER_H +#define BT_KRYLOV_SOLVER_H +#include +#include +#include +#include +#include +#include +#include "LinearMath/btQuickprof.h" + +template +class btKrylovSolver +{ + typedef btAlignedObjectArray TVStack; +public: + int m_maxIterations; + btScalar m_tolerance; + btKrylovSolver(int maxIterations, btScalar tolerance) + : m_maxIterations(maxIterations) + , m_tolerance(tolerance) + { + + } + + virtual ~btKrylovSolver(){} + + virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0; + + virtual void reinitialize(const TVStack& b) = 0; + + virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b) + { + // c = a-b + btAssert(a.size() == b.size()); + TVStack c; + c.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + { + c[i] = a[i] - b[i]; + } + return c; + } + + virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a) + { + return dot(a,a); + } + + virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a) + { + btScalar ret = 0; + for (int i = 0; i < a.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + ret = btMax(ret, btFabs(a[i][d])); + } + } + return ret; + } + + virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b) + { + btScalar ans(0); + for (int i = 0; i < a.size(); ++i) + ans += a[i].dot(b[i]); + return ans; + } + + virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) + { + // result += s*a + btAssert(a.size() == result.size()); + for (int i = 0; i < a.size(); ++i) + result[i] += s * a[i]; + } + + virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) + { + // result = a*s + b + TVStack result; + result.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + result[i] = s * a[i] + b[i]; + return result; + } + + virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance) + { + m_tolerance = tolerance; + } +}; +#endif /* BT_KRYLOV_SOLVER_H */