Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-09-29 05:26:00 +00:00
Code Issues Packages Projects Releases Wiki Activity

add full preconditioner for KKT system

Browse Source
This commit is contained in:
Xuchen Han
2020-04-15 13:32:16 -07:00
parent 8783b4411c
commit 26ef0e093a
6 changed files with 119 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
src/BulletSoftBody/btConjugateResidual.h
View File

@@ -26,21 +26,21 @@ template <class MatrixX>
class btConjugateResidual
{
typedef btAlignedObjectArray<btVector3> 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)

1
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@@ -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]);
}
}

5
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -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<btSoftBody *>& softBodies, btScalar dt)

2
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@@ -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) ;

1
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@@ -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

95
src/BulletSoftBody/btPreconditioner.h
View File

@@ -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 */