Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

switch to Baraff style constraint

Browse Source
This commit is contained in:
Xuchen Han
2019-08-09 13:39:25 -07:00
parent 27492887bf
commit 5b8df6a708
5 changed files with 98 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
src/BulletSoftBody/btConjugateGradient.h
View File

@@ -32,62 +32,108 @@ public:
virtual ~btConjugateGradient(){}
// // return the number of iterations taken
// int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance)
// {
// BT_PROFILE("CGSolve");
// btAssert(x.size() == b.size());
// reinitialize(b);
//
// // r = b - A * x --with assigned dof zeroed out
// A.multiply(x, temp);
// r = sub(b, temp);
// A.project(r);
//
// btScalar r_norm = std::sqrt(squaredNorm(r));
// if (r_norm < tolerance) {
// std::cout << "Iteration = 0" << std::endl;
// std::cout << "Two norm of the residual = " << r_norm << std::endl;
// return 0;
// }
//
// // z = M^(-1) * r
// A.precondition(r, z);
// p = z;
// // temp = A*p
// A.multiply(p, temp);
// A.project(temp);
// btScalar r_dot_z = dot(z,r), r_dot_z_new;
// // alpha = r^T * z / (p^T * A * p)
// btScalar alpha = r_dot_z / dot(p, temp), beta;
//
// for (int k = 1; k < max_iterations; k++) {
// // x += alpha * p;
// // r -= alpha * temp;
// multAndAddTo(alpha, p, x);
// multAndAddTo(-alpha, temp, r);
// // zero out the dofs of r
// A.project(r);
//// A.enforceConstraint(x);
// r_norm = std::sqrt(squaredNorm(r));
//
// if (r_norm < tolerance) {
// std::cout << "ConjugateGradient iterations " << k << std::endl;
// return k;
// }
//
// // z = M^(-1) * r
// A.precondition(r, z);
// r_dot_z_new = dot(r,z);
// beta = r_dot_z_new/ r_dot_z;
// r_dot_z = r_dot_z_new;
// // p = z + beta * p;
// p = multAndAdd(beta, p, z);
// // temp = A * p;
// A.multiply(p, temp);
// A.project(temp);
// // alpha = r^T * z / (p^T * A * p)
// alpha = r_dot_z / dot(p, temp);
// }
// std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
// return max_iterations;
// }
// return the number of iterations taken
int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance)
{
BT_PROFILE("CGSolve");
btAssert(x.size() == b.size());
reinitialize(b);
// r = b - A * x --with assigned dof zeroed out
A.multiply(x, temp);
r = sub(b, temp);
A.project(r);
A.enforceConstraint(x);
btScalar r_norm = std::sqrt(squaredNorm(r));
if (r_norm < tolerance) {
std::cout << "Iteration = 0" << std::endl;
std::cout << "Two norm of the residual = " << r_norm << std::endl;
return 0;
}
// z = M^(-1) * r
A.precondition(r, z);
A.project(z);
btScalar r_dot_z = dot(z,r);
if (r_dot_z < tolerance) {
std::cout << "Iteration = 0" << std::endl;
std::cout << "Two norm of the residual = " << r_dot_z << std::endl;
return 0;
}
p = z;
// temp = A*p
A.multiply(p, temp);
btScalar r_dot_z = dot(z,r), r_dot_z_new;
// alpha = r^T * z / (p^T * A * p)
btScalar alpha = r_dot_z / dot(p, temp), beta;
btScalar r_dot_z_new = r_dot_z;
for (int k = 1; k < max_iterations; k++) {
// temp = A*p
A.multiply(p, temp);
A.project(temp);
// alpha = r^T * z / (p^T * A * p)
btScalar alpha = r_dot_z_new / dot(p, temp);
// x += alpha * p;
// r -= alpha * temp;
multAndAddTo(alpha, p, x);
// r -= alpha * temp;
multAndAddTo(-alpha, temp, r);
// zero out the dofs of r
A.project(r);
A.enforceConstraint(x);
r_norm = std::sqrt(squaredNorm(r));
if (r_norm < tolerance) {
// 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) {
std::cout << "ConjugateGradient iterations " << k << std::endl;
return k;
}
// z = M^(-1) * r
A.precondition(r, z);
r_dot_z_new = dot(r,z);
beta = r_dot_z_new/ r_dot_z;
r_dot_z = r_dot_z_new;
// p = z + beta * p;
btScalar beta = r_dot_z_new/ r_dot_z;
p = multAndAdd(beta, p, z);
// temp = A * p;
A.multiply(p, temp);
// alpha = r^T * z / (p^T * A * p)
alpha = r_dot_z / dot(p, temp);
}
std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
return max_iterations;

2
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@@ -73,7 +73,6 @@ public:
{
BT_PROFILE("enforceConstraint");
projection.enforceConstraint(x);
updateVelocity(x);
}
// add dv to velocity
@@ -86,7 +85,6 @@ public:
void project(TVStack& r)
{
BT_PROFILE("project");
projection.update();
projection.project(r);
}

14
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -31,20 +31,20 @@ btDeformableBodySolver::~btDeformableBodySolver()
void btDeformableBodySolver::solveConstraints(float solverdt)
{
BT_PROFILE("solveConstraints");
// add constraints to the solver
setConstraints();
// save v_{n+1}^* velocity after explicit forces
backupVelocity();
// add constraints to the solver
setConstraints();
m_objective->computeResidual(solverdt, m_residual);
computeStep(m_dv, m_residual);
updateVelocity();
}
void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual)
{
btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * m_objective->computeNorm(residual);
m_cg.solve(*m_objective, dv, residual, tolerance);
}
@@ -75,6 +75,12 @@ void btDeformableBodySolver::setConstraints()
{
BT_PROFILE("setConstraint");
m_objective->setConstraints();
for (int i = 0; i < 10; ++i)
{
m_objective->projection.update();
m_objective->enforceConstraint(m_dv);
m_objective->updateVelocity(m_dv);
}
}
void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)