From c735830f0a71e9339fcbe3e21d4e13179daf8f05 Mon Sep 17 00:00:00 2001 From: jingyuc Date: Tue, 24 Aug 2021 15:08:57 -0400 Subject: [PATCH] more stable rigid contact --- examples/ReducedDeformableDemo/FreeFall.cpp | 2 +- .../btReducedDeformableContactConstraint.cpp | 29 ++++++++++++------- .../btReducedDeformableContactConstraint.h | 4 +-- .../btDeformableMultiBodyConstraintSolver.cpp | 3 ++ src/BulletSoftBody/btSoftBodyInternals.h | 2 +- 5 files changed, 26 insertions(+), 14 deletions(-) diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp index 0c0323423..538415a13 100644 --- a/examples/ReducedDeformableDemo/FreeFall.cpp +++ b/examples/ReducedDeformableDemo/FreeFall.cpp @@ -161,7 +161,7 @@ void FreeFall::initPhysics() getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.1); // rsb->scale(btVector3(1, 1, 1)); - rsb->translate(btVector3(0, 4.5, 0)); //TODO: add back translate and scale + rsb->translate(btVector3(0, 10, 0)); //TODO: add back translate and scale // rsb->setTotalMass(0.5); rsb->setStiffnessScale(1); rsb->setDamping(damping_alpha, damping_beta); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp index 2ee931a1b..4bec0c129 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp @@ -57,8 +57,8 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr m_contactNormalA = c.m_cti.m_normal; m_contactNormalB = -c.m_cti.m_normal; - m_impulseFactorInv = c.m_c0; - m_normalImpulseFactorInv = (m_impulseFactorInv * m_contactNormalA).dot(m_contactNormalA); + m_impulseFactor = c.m_c0; + m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA); } btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) @@ -80,7 +80,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont // v_rel_normal += m_penetration / infoGlobal.m_timeStep; // add penetration correction vel // } - btScalar deltaImpulse = m_rhs - deltaV_rel_normal * m_normalImpulseFactorInv; + btScalar deltaImpulse = m_rhs - deltaV_rel_normal / m_normalImpulseFactor; // btScalar deltaImpulse = m_rhs - v_rel_normal * m_normalImpulseFactorInv; // cumulative impulse that has been applied @@ -110,7 +110,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont // return 0; // } // btScalar residualSquare = v_rel_normal * v_rel_normal; // get residual - btScalar residualSquare = deltaImpulse / m_normalImpulseFactorInv; // get residual + btScalar residualSquare = deltaImpulse * m_normalImpulseFactor; // get residual residualSquare *= residualSquare; // // compute the tangential relative vel @@ -202,15 +202,23 @@ void btReducedDeformableNodeRigidContactConstraint::warmStarting() m_bufferVelocityA = va; m_bufferVelocityB = vb; - // get relative velocity and magnitude + // we define the (+) direction of errors to be the outward surface normal of the rigid object btVector3 v_rel = va - vb; - btScalar v_rel_normal = -btDot(v_rel, m_contactNormalA); + btScalar velocity_error = -btDot(v_rel, m_contactNormalA); // magnitude of relative velocity + btScalar position_error = 0; + std::cout << "penetration = " << m_penetration << "\n"; if (m_penetration > 0) { - v_rel_normal += m_penetration / m_dt; // add penetration correction vel + // velocity_error += m_penetration / m_dt; // TODO: why? } - - m_rhs = -v_rel_normal * m_normalImpulseFactorInv; + else + { + // add penetration correction vel + position_error = m_penetration * btScalar(0.2) / m_dt; + } + // get the initial estimate of impulse magnitude to be applied + // m_rhs = -velocity_error * m_normalImpulseFactorInv; + m_rhs = -(velocity_error + position_error) / m_normalImpulseFactor; } btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const @@ -240,7 +248,8 @@ void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3 // m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt); // m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform()); std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n'; - // std::cout << "node: " << m_node->index << " m_x: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n'; + btVector3 v_after = getDeltaVb() + m_node->m_v; + std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n'; } // ================= face vs rigid constraints =================== diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h index 15d8f8aad..dd0400139 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h @@ -38,14 +38,14 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac btScalar m_appliedTangentImpulse; btScalar m_impulseFactorNormal; btScalar m_impulseFactorTangent; - btScalar m_normalImpulseFactorInv; + btScalar m_normalImpulseFactor; btScalar m_rhs; btVector3 m_contactNormalA; // for rigid body btVector3 m_contactNormalB; // for reduced deformable body btVector3 m_relPosA; // relative position of the contact point for A btVector3 m_relPosB; // relative position of the contact point for B - btMatrix3x3 m_impulseFactorInv; // total inverse impulse matrix + btMatrix3x3 m_impulseFactor; // total impulse matrix btVector3 m_bufferVelocityA; // velocity at the beginning of the iteration btVector3 m_bufferVelocityB; diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index c8e537f54..68a6b9e90 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -15,6 +15,8 @@ #include "btDeformableMultiBodyConstraintSolver.h" #include +#include +#include // override the iterations method to include deformable/multibody contact btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { @@ -62,6 +64,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b } } std::cout << "======next step=========\n"; + // std::this_thread::sleep_for (std::chrono::milliseconds(100)); } return 0.f; } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index b6ab39721..1468ed496 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1695,7 +1695,7 @@ struct btSoftColliders if (psb->m_reducedModel) { - c.m_c0 = (psb->getImpulseFactor(n.index)).inverse(); + c.m_c0 = psb->getImpulseFactor(n.index); //impulse factor K (not the inverse) // c.m_c1 = n.m_x - psb->getRigidTransform().getOrigin(); } else