summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btConjugateResidual.h')
-rw-r--r--thirdparty/bullet/BulletSoftBody/btConjugateResidual.h256
1 files changed, 90 insertions, 166 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h b/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h
index 7b211c4172..6146120365 100644
--- a/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h
+++ b/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h
@@ -15,174 +15,98 @@
#ifndef BT_CONJUGATE_RESIDUAL_H
#define BT_CONJUGATE_RESIDUAL_H
-#include <iostream>
-#include <cmath>
-#include <limits>
-#include <LinearMath/btAlignedObjectArray.h>
-#include <LinearMath/btVector3.h>
-#include <LinearMath/btScalar.h>
-#include "LinearMath/btQuickprof.h"
+#include "btKrylovSolver.h"
+
template <class MatrixX>
-class btConjugateResidual
+class btConjugateResidual : public btKrylovSolver<MatrixX>
{
- typedef btAlignedObjectArray<btVector3> TVStack;
- 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;
+ typedef btAlignedObjectArray<btVector3> TVStack;
+ typedef btKrylovSolver<MatrixX> 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
+ btScalar best_r;
+
public:
- btConjugateResidual(const int max_it_in)
- : max_iterations(max_it_in)
- {
- tolerance_squared = 1e-2;
- }
-
- virtual ~btConjugateResidual(){}
-
- // return the number of iterations taken
- int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
- {
- BT_PROFILE("CRSolve");
- btAssert(x.size() == b.size());
- 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);
- // 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) {
- if (verbose)
- {
- std::cout << "Iteration = 0" << std::endl;
- std::cout << "Two norm of the residual = " << residual_norm << std::endl;
- }
- return 0;
- }
- p = r;
- btScalar r_dot_Ar, r_dot_Ar_new;
- // temp_p = A*p
- 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++) {
- // 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);
- // x += alpha * p;
- multAndAddTo(alpha, p, x);
- // r -= alpha * z;
- multAndAddTo(-alpha, z, r);
- btScalar norm_r = norm(r);
- if (norm_r < best_r)
- {
- best_x = x;
- best_r = norm_r;
- if (norm_r < tolerance_squared) {
- if (verbose)
- {
- std::cout << "ConjugateResidual iterations " << k << std::endl;
- }
- return k;
- }
- else
- {
- if (verbose)
- {
- std::cout << "ConjugateResidual iterations " << k << " has residual "<< norm_r << std::endl;
- }
- }
- }
- // temp_r = A * r;
- A.multiply(r, temp_r);
- r_dot_Ar_new = 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);
- // temp_p = beta*temp_p + temp_r;
- temp_p = multAndAdd(beta, temp_p, temp_r);
- }
- if (verbose)
- {
- std::cout << "ConjugateResidual max iterations reached " << max_iterations << std::endl;
- }
- x = best_x;
- return max_iterations;
- }
-
- void reinitialize(const TVStack& b)
- {
- r.resize(b.size());
- p.resize(b.size());
- 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)
- {
- // 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;
- }
+ btConjugateResidual(const int max_it_in)
+ : Base(max_it_in, 1e-8)
+ {
+ }
+
+ virtual ~btConjugateResidual() {}
+
+ // return the number of iterations taken
+ int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
+ {
+ BT_PROFILE("CRSolve");
+ btAssert(x.size() == b.size());
+ 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 = this->sub(b, temp_r);
+ // z = M^(-1) * r
+ A.precondition(r, z); // borrow z to store preconditioned r
+ r = z;
+ btScalar residual_norm = this->norm(r);
+ if (residual_norm <= Base::m_tolerance)
+ {
+ return 0;
+ }
+ p = r;
+ btScalar r_dot_Ar, r_dot_Ar_new;
+ // temp_p = A*p
+ A.multiply(p, temp_p);
+ // temp_r = A*r
+ temp_r = temp_p;
+ 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 / this->dot(temp_p, z);
+ // x += alpha * p;
+ this->multAndAddTo(alpha, p, x);
+ // r -= alpha * z;
+ 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 < Base::m_tolerance)
+ {
+ return k;
+ }
+ }
+ // temp_r = A * r;
+ A.multiply(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 = this->multAndAdd(beta, p, r);
+ // temp_p = 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 Base::m_maxIterations;
+ }
+
+ void reinitialize(const TVStack& b)
+ {
+ r.resize(b.size());
+ p.resize(b.size());
+ z.resize(b.size());
+ temp_p.resize(b.size());
+ temp_r.resize(b.size());
+ best_x.resize(b.size());
+ best_r = SIMD_INFINITY;
+ }
};
#endif /* btConjugateResidual_h */
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-07 19:03:24 +0000