From 5b8df6a708b7e13fa50b67bf2182a85797520440 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 9 Aug 2019 13:39:25 -0700 Subject: [PATCH] switch to Baraff style constraint --- examples/DeformableDemo/DeformableRigid.cpp | 6 +- examples/DeformableDemo/GraspDeformable.cpp | 7 +- src/BulletSoftBody/btConjugateGradient.h | 116 ++++++++++++------ .../btDeformableBackwardEulerObjective.h | 2 - src/BulletSoftBody/btDeformableBodySolver.cpp | 14 ++- 5 files changed, 98 insertions(+), 47 deletions(-) diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 403ce4907..9268572ef 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -228,11 +228,11 @@ void DeformableRigid::initPhysics() psb->getCollisionShape()->setMargin(0.1); psb->generateBendingConstraints(2); psb->setTotalMass(1); - psb->setSpringStiffness(1); - psb->setDampingCoefficient(0.01); + psb->setSpringStiffness(2); + psb->setDampingCoefficient(0.05); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 1; + psb->m_cfg.kDF = 0; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index dc7de0057..15348af26 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -331,14 +331,15 @@ void GraspDeformable::initPhysics() psb->getCollisionShape()->setMargin(0.1); psb->setTotalMass(1); psb->setSpringStiffness(0); - psb->setDampingCoefficient(0.04); + psb->setDampingCoefficient(0.02); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 2; + psb->m_cfg.kDF = 0; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(2,2)); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(1,1)); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 22a9f3ada..d7e98761c 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -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; diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 2f44bf016..f95cb05aa 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -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); } diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index eb8e62237..2b2827e88 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -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::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)