diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 08af5542d..830667448 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -26,16 +26,18 @@ class btConjugateGradient typedef btAlignedObjectArray TVStack; TVStack r,p,z,temp; int max_iterations; + btScalar tolerance; public: btConjugateGradient(const int max_it_in) : max_iterations(max_it_in) { + tolerance = 1024 * std::numeric_limits::epsilon(); } virtual ~btConjugateGradient(){} // return the number of iterations taken - int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance, bool verbose = false) + int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar relative_tolerance, bool verbose = false) { BT_PROFILE("CGSolve"); btAssert(x.size() == b.size()); @@ -48,7 +50,8 @@ public: A.precondition(r, z); A.project(z); btScalar r_dot_z = dot(z,r); - if (dot(z,z) < tolerance) { + btScalar local_tolerance = btMin(relative_tolerance * std::sqrt(r_dot_z), tolerance); + if (std::sqrt(r_dot_z) < local_tolerance) { if (verbose) { std::cout << "Iteration = 0" << std::endl; @@ -58,11 +61,21 @@ public: } p = z; btScalar r_dot_z_new = r_dot_z; - for (int k = 1; k < max_iterations; k++) { + 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) + if (dot(p,temp) < 0) + { + if (verbose) + std::cout << "Encountered negative direction in CG!"<totalElasticEnergy(); + } + return e; } void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index e96738557..c57f2e097 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -124,6 +124,8 @@ public: { m_implicit = implicit; } + + btScalar totalEnergy(); }; #endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index a902322ce..b85255220 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -23,6 +23,11 @@ btDeformableBodySolver::btDeformableBodySolver() , m_cg(20) , m_maxNewtonIterations(5) , m_newtonTolerance(1e-4) +//, m_lineSearch(false) +//, m_cg(10) +//, m_maxNewtonIterations(5) +//, m_newtonTolerance(1e-3) +, m_lineSearch(true) { m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity); } @@ -63,13 +68,37 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) } m_objective->computeResidual(solverdt, m_residual); - if (m_objective->computeNorm(m_residual) < m_newtonTolerance) + if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) { break; } m_objective->applyDynamicFriction(m_residual); - computeStep(m_ddv, m_residual); - updateDv(); + if (m_lineSearch) + { + btScalar inner_product = computeDescentStep(m_ddv,m_residual); + btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 + btScalar scale = 2; + btScalar f0 = m_objective->totalEnergy()+kineticEnergy(), f1, f2; + backupDv(); + do { + scale *= beta; + if (scale < 1e-8) { + //std::cout << "Could not find sufficient descent!" << std::endl; + return; + } + updateEnergy(scale); + f1 = m_objective->totalEnergy()+kineticEnergy(); + f2 = f0 - alpha * scale * inner_product; + } while (!(f1 < f2)); // if anything here is nan then the search continues + revertDv(); + updateDv(scale); + } + else + { + computeStep(m_ddv, m_residual); + updateDv(); + } + for (int j = 0; j < m_numNodes; ++j) { m_ddv[j].setZero(); @@ -79,26 +108,99 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) } } +btScalar btDeformableBodySolver::kineticEnergy() +{ + btScalar ke = 0; + for (int i = 0; i < m_softBodySet.size();++i) + { + btSoftBody* psb = m_softBodySet[i]; + for (int j = 0; j < psb->m_nodes.size();++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + ke += m_dv[node.index].length2() * 0.5 / node.m_im; + } + } + } + return ke; +} + +void btDeformableBodySolver::backupDv() +{ + m_backup_dv.resize(m_dv.size()); + for (int i = 0; icomputeNorm(residual), m_newtonTolerance))); + m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false); + btScalar inner_product = m_cg.dot(residual, m_ddv); + btScalar tol = 1e-5 * m_objective->computeNorm(residual) * m_objective->computeNorm(m_ddv); + if (inner_product < -tol) + { + std::cout << "Looking backwards!" << std::endl; + for (int i = 0; i < m_ddv.size();++i) + { + m_ddv[i] = -m_ddv[i]; + } + inner_product = -inner_product; + } + else if (std::abs(inner_product) < tol) + { + std::cout << "Gradient Descent!" << std::endl; + btScalar res_norm = m_objective->computeNorm(residual); + btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; + for (int i = 0; i < m_ddv.size();++i) + { + m_ddv[i] = scale * residual[i]; + } + inner_product = scale * res_norm * res_norm; + } + return inner_product; +} + void btDeformableBodySolver::updateState() { updateVelocity(); updateTempPosition(); - } -void btDeformableBodySolver::updateDv() +void btDeformableBodySolver::updateDv(btScalar scale) { for (int i = 0; i < m_numNodes; ++i) { - m_dv[i] += m_ddv[i]; + m_dv[i] += scale * m_ddv[i]; } } void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { //btScalar tolerance = std::numeric_limits::epsilon() * m_objective->computeNorm(residual); - btScalar tolerance = std::numeric_limits::epsilon(); - m_cg.solve(*m_objective, ddv, residual, tolerance); + btScalar relative_tolerance = btMin(0.5, std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance))); + m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false); } void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies, btScalar dt) diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index fed15b2b2..e0629466a 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -34,6 +34,7 @@ class btDeformableBodySolver : public btSoftBodySolver protected: int m_numNodes; TVStack m_dv; + TVStack m_backup_dv; TVStack m_ddv; TVStack m_residual; btAlignedObjectArray m_softBodySet; @@ -45,6 +46,7 @@ protected: bool m_implicit; int m_maxNewtonIterations; btScalar m_newtonTolerance; + bool m_lineSearch; public: btDeformableBackwardEulerObjective* m_objective; @@ -82,6 +84,7 @@ public: bool updateNodes(); void computeStep(TVStack& dv, const TVStack& residual); + btScalar computeDescentStep(TVStack& ddv, const TVStack& residual); virtual void predictMotion(btScalar solverdt); @@ -103,9 +106,13 @@ public: void updateState(); - void updateDv(); + void updateDv(btScalar scale = 1); void updateTempPosition(); + void backupDv(); + void revertDv(); + void updateEnergy(btScalar scale); + btScalar kineticEnergy(); }; #endif /* btDeformableBodySolver_h */ diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 6fd53144a..f5b4c2de9 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -72,6 +72,24 @@ public: { return BT_GRAVITY_FORCE; } + + virtual double totalElasticEnergy() + { + double e = 0; + for (int i = 0; im_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + e -= m_gravity.dot(node.m_q)/node.m_im; + } + } + } + return e; + } };