diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index 1274a3721..5b342aada 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -206,15 +206,15 @@ void DeformableMultibody::initPhysics() psb->getCollisionShape()->setMargin(0.25); psb->generateBendingConstraints(2); - psb->setTotalMass(5); + psb->setTotalMass(1); 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 = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb->setCollisionFlags(0); getDeformableDynamicsWorld()->addSoftBody(psb); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30, 1, true); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index f6c087d2a..47ac3d9ff 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -219,7 +219,7 @@ void DeformableRigid::initPhysics() psb->setTotalMass(1); 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 = .4; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 475fa9095..e7d63d52d 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -275,12 +275,12 @@ void GraspDeformable::initPhysics() { char absolute_path[1024]; b3BulletDefaultFileIO fileio; - fileio.findResourcePath("ditto.vtk", absolute_path, 1024); + fileio.findResourcePath("ditto.vtk", absolute_path, 1024); // fileio.findResourcePath("banana.vtk", absolute_path, 1024); // fileio.findResourcePath("ball.vtk", absolute_path, 1024); // fileio.findResourcePath("deformable_crumpled_napkin_sim.vtk", absolute_path, 1024); // fileio.findResourcePath("single_tet.vtk", absolute_path, 1024); - // fileio.findResourcePath("tube.vtk", absolute_path, 1024); +// fileio.findResourcePath("tube.vtk", absolute_path, 1024); // fileio.findResourcePath("torus.vtk", absolute_path, 1024); // fileio.findResourcePath("paper_roll.vtk", absolute_path, 1024); // fileio.findResourcePath("bread.vtk", absolute_path, 1024); diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index ed811a417..07c491d89 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -231,7 +231,7 @@ void Pinch::initPhysics() btVector3 gravity = btVector3(0, -10, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; - + getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25); getDeformableDynamicsWorld()->setSolverCallback(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index a07e36c77..e500c412c 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -425,12 +425,12 @@ public: m_robotSim.setGravity(btVector3(0, 0, -10)); b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006); - args.m_springElasticStiffness = .1; - args.m_springDampingStiffness = .0004; - args.m_springBendingStiffness = 1; - args.m_frictionCoeff = 1; + args.m_springElasticStiffness = 1; + args.m_springDampingStiffness = .01; + args.m_springBendingStiffness = .1; + args.m_frictionCoeff = 10; args.m_useSelfCollision = false; -// args.m_useFaceContact = true; + args.m_useFaceContact = true; args.m_useBendingSprings = true; args.m_startPosition.setValue(0, 0, 0); args.m_startOrientation.setValue(0, 0, 1, 1); @@ -476,7 +476,7 @@ public: revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1); m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2); - m_robotSim.setNumSimulationSubSteps(8); + m_robotSim.setNumSimulationSubSteps(2); } if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2d9c25eb8..0aa4ca89a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8153,7 +8153,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar { spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness; } - btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false, spring_bending_stiffness); + btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness); deformWorld->addForce(psb, springForce); m_data->m_lf.push_back(springForce); } diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 1bb21104c..b5f4a3c86 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -139,7 +139,12 @@ public: if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { - btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, childTrans, -1, index); + btTransform preTransform = childTrans; + if (this->m_compoundColObjWrap->m_preTransform) + { + preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform)); + } + btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index); btCollisionAlgorithm* algo = 0; bool allocatedAlgorithm = false; diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index e82d1b139..50c09d83e 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -46,7 +46,7 @@ struct btContactSolverInfoData btScalar m_sor; //successive over-relaxation term btScalar m_erp; //error reduction for non-contact constraints btScalar m_erp2; //error reduction for contact constraints - btScalar m_deformable_erp; //error reduction for deformable constraints + btScalar m_deformable_erp; //error reduction for deformable constraints btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts btScalar m_frictionERP; //error reduction for friction constraints btScalar m_frictionCFM; //constraint force mixing for friction constraints @@ -82,7 +82,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); - m_deformable_erp = btScalar(0.); + m_deformable_erp = btScalar(0.3); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 1b247641a..eb50f5a56 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -186,9 +186,9 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack } //set constraints as projections -void btDeformableBackwardEulerObjective::setConstraints() +void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal) { - m_projection.setConstraints(); + m_projection.setConstraints(infoGlobal); } void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 05ab42ff0..89cbb38c5 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -79,7 +79,7 @@ public: void updateVelocity(const TVStack& dv); //set constraints as projections - void setConstraints(); + void setConstraints(const btContactSolverInfo& infoGlobal); // update the projections and project the residual void project(TVStack& r) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 7724a8ec6..6404ce6ee 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -228,16 +228,16 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArrayreinitialize(nodeUpdated, dt); } -void btDeformableBodySolver::setConstraints() +void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) { BT_PROFILE("setConstraint"); - m_objective->setConstraints(); + m_objective->setConstraints(infoGlobal); } -btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies) +btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("solveContactConstraints"); - btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies); + btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal); return maxSquaredResidual; } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index f78a8f696..bce0d30b3 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -65,7 +65,7 @@ public: virtual void solveDeformableConstraints(btScalar solverdt); // solve the contact between deformable and rigid as well as among deformables - btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies); + btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); // solve the position error between deformable and rigid as well as among deformables; btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); @@ -77,7 +77,7 @@ public: void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); // set up contact constraints - void setConstraints(); + void setConstraints(const btContactSolverInfo& infoGlobal); // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion virtual void predictMotion(btScalar solverdt); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index e8219dc50..3a5dfc88d 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -15,9 +15,9 @@ #include "btDeformableContactConstraint.h" /* ================ Deformable Node Anchor =================== */ -btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a) +btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal) : m_anchor(&a) -, btDeformableContactConstraint(a.m_cti.m_normal) +, btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal) { } @@ -79,7 +79,7 @@ btVector3 btDeformableNodeAnchorConstraint::getVa() const return va; } -btScalar btDeformableNodeAnchorConstraint::solveConstraint() +btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { const btSoftBody::sCti& cti = m_anchor->m_cti; btVector3 va = getVa(); @@ -134,14 +134,14 @@ void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse) } /* ================ Deformable vs. Rigid =================== */ -btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c) +btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal) : m_contact(&c) -, btDeformableContactConstraint(c.m_cti.m_normal) +, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal) { m_total_normal_dv.setZero(); m_total_tangent_dv.setZero(); - // penetration is non-positive. The magnitude of penetration is the depth of penetration. - m_penetration = btMin(btScalar(0), c.m_cti.m_offset); + // The magnitude of penetration is the depth of penetration. + m_penetration = btMin(btScalar(0),c.m_cti.m_offset); } btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) @@ -206,16 +206,16 @@ btVector3 btDeformableRigidContactConstraint::getVa() const return va; } -btScalar btDeformableRigidContactConstraint::solveConstraint() +btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { const btSoftBody::sCti& cti = m_contact->m_cti; btVector3 va = getVa(); btVector3 vb = getVb(); btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal); + const 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; + btVector3 impulse = m_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ; const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); btVector3 impulse_tangent = impulse - impulse_normal; btVector3 old_total_tangent_dv = m_total_tangent_dv; @@ -256,6 +256,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint() impulse = impulse_normal + impulse_tangent; // apply impulse to deformable nodes involved and change their velocities applyImpulse(impulse); + if (residualSquare < 1e-7) + return residualSquare; // apply impulse to the rigid/multibodies involved and change their velocities if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { @@ -319,9 +321,9 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo } /* ================ Node vs. Rigid =================== */ -btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact) +btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal) : m_node(contact.m_node) - , btDeformableRigidContactConstraint(contact) + , btDeformableRigidContactConstraint(contact, infoGlobal) { } @@ -357,9 +359,9 @@ void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& }; /* ================ Face vs. Rigid =================== */ -btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact) +btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal) : m_face(contact.m_face) -, btDeformableRigidContactConstraint(contact) +, btDeformableRigidContactConstraint(contact, infoGlobal) { } @@ -411,19 +413,60 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul v1 -= dv * contact->m_weights[1]; if (im2 > 0) v2 -= dv * contact->m_weights[2]; - - // apply strain limiting to prevent undamped modes - btScalar m01 = (btScalar(1)/(im0 + im1)); - btScalar m02 = (btScalar(1)/(im0 + im2)); - btScalar m12 = (btScalar(1)/(im1 + im2)); - - btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); - btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); - btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); - - v0 += dv0; - v1 += dv1; - v2 += dv2; + + btScalar relaxation = 1./btScalar(m_infoGlobal->m_numIterations); + btScalar m01 = (relaxation/(im0 + im1)); + btScalar m02 = (relaxation/(im0 + im2)); + btScalar m12 = (relaxation/(im1 + im2)); + #ifdef USE_STRAIN_RATE_LIMITING + // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%. + btScalar p = 0.01; + btVector3& x0 = face->m_n[0]->m_x; + btVector3& x1 = face->m_n[1]->m_x; + btVector3& x2 = face->m_n[2]->m_x; + const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1}; + const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1}; + btVector3 u[3]; + btScalar x_diff_dot_u, dn[3]; + btScalar dt = m_infoGlobal->m_timeStep; + for (int i = 0; i < 3; ++i) + { + btScalar x_diff_norm = x_diff[i].safeNorm(); + btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm(); + btScalar strainRate = x_diff_norm_new/x_diff_norm; + u[i] = v_diff[i]; + u[i].safeNormalize(); + if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p)) + { + dn[i] = 0; + continue; + } + x_diff_dot_u = btDot(x_diff[i], u[i]); + btScalar s; + if (1-p > strainRate) + { + s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm)); + } + else + { + s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm)); + } + // x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm(); + // strainRate = x_diff_norm_new/x_diff_norm; + dn[i] = s - v_diff[i].safeNorm(); + } + btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1])); + btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2])); + btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1])); +#else + // apply strain limiting to prevent undamped modes + btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); + btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); + btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); +#endif + v0 += dv0; + v1 += dv1; + v2 += dv2; } void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) @@ -447,11 +490,11 @@ void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& } /* ================ Face vs. Node =================== */ -btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact) +btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal) : m_node(contact.m_node) , m_face(contact.m_face) , m_contact(&contact) -, btDeformableContactConstraint(contact.m_normal) +, btDeformableContactConstraint(contact.m_normal, infoGlobal) { m_total_normal_dv.setZero(); m_total_tangent_dv.setZero(); @@ -487,7 +530,7 @@ btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n return dv * contact->m_weights[2]; } -btScalar btDeformableFaceNodeContactConstraint::solveConstraint() +btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { btVector3 va = getVa(); btVector3 vb = getVb(); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index 912119e7c..7acf9aa69 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -24,31 +24,33 @@ public: // True if the friction is static // False if the friction is dynamic bool m_static; - - // normal of the contact - btVector3 m_normal; - - btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal) - { - } - - btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal) - { - } - - btDeformableContactConstraint(const btDeformableContactConstraint& other) - : m_static(other.m_static) - , m_normal(other.m_normal) - { - - } - btDeformableContactConstraint(){} - + const btContactSolverInfo* m_infoGlobal; + + // normal of the contact + btVector3 m_normal; + + btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) + { + } + + btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) + { + } + + btDeformableContactConstraint(){} + + btDeformableContactConstraint(const btDeformableContactConstraint& other) + : m_static(other.m_static) + , m_normal(other.m_normal) + , m_infoGlobal(other.m_infoGlobal) + { + } + virtual ~btDeformableContactConstraint(){} // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact - virtual btScalar solveConstraint() = 0; + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; // solve the position error by applying an inelastic impulse that changes only the position (not velocity) virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0; @@ -79,22 +81,19 @@ class btDeformableStaticConstraint : public btDeformableContactConstraint public: const btSoftBody::Node* m_node; - btDeformableStaticConstraint(){} - - btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0)) + btDeformableStaticConstraint(const btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal) { } - + btDeformableStaticConstraint(){} btDeformableStaticConstraint(const btDeformableStaticConstraint& other) : m_node(other.m_node) , btDeformableContactConstraint(other) { - } virtual ~btDeformableStaticConstraint(){} - virtual btScalar solveConstraint() + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) { return 0; } @@ -130,14 +129,14 @@ class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint { public: const btSoftBody::DeformableNodeRigidAnchor* m_anchor; - - btDeformableNodeAnchorConstraint(){} - btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c); + + btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal); btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); + btDeformableNodeAnchorConstraint(){} virtual ~btDeformableNodeAnchorConstraint() { } - virtual btScalar solveConstraint(); + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) { // todo xuchenhan@ @@ -169,10 +168,10 @@ public: btVector3 m_total_tangent_dv; btScalar m_penetration; const btSoftBody::DeformableRigidContact* m_contact; - - btDeformableRigidContactConstraint(){} - btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c); + + btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); + btDeformableRigidContactConstraint(){} virtual ~btDeformableRigidContactConstraint() { } @@ -180,7 +179,7 @@ public: // object A is the rigid/multi body, and object B is the deformable node/face virtual btVector3 getVa() const; - virtual btScalar solveConstraint(); + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); @@ -197,11 +196,10 @@ class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactCo public: // the deformable node in contact const btSoftBody::Node* m_node; - - btDeformableNodeRigidContactConstraint(){} - btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact); + + btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); - + btDeformableNodeRigidContactConstraint(){} virtual ~btDeformableNodeRigidContactConstraint() { } @@ -228,10 +226,9 @@ class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactCo { public: const btSoftBody::Face* m_face; - btDeformableFaceRigidContactConstraint(){} - btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact); + btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal); btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); - + btDeformableFaceRigidContactConstraint(){} virtual ~btDeformableFaceRigidContactConstraint() { } @@ -263,13 +260,11 @@ public: btVector3 m_total_normal_dv; btVector3 m_total_tangent_dv; - btDeformableFaceNodeContactConstraint(){} - - btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact); - + btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableFaceNodeContactConstraint(){} virtual ~btDeformableFaceNodeContactConstraint(){} - virtual btScalar solveConstraint(); + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) { diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 5a4f3241b..a59177e16 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,7 +17,7 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include #include -btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies) +btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; for (int i = 0; i < numDeformableBodies; ++i) @@ -32,25 +32,25 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) { btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k) { btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) { btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } for (int k = 0; k < m_deformableConstraints[j].size(); ++k) { btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } } @@ -108,7 +108,7 @@ btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverI return residualSquare; } -void btDeformableContactProjection::setConstraints() +void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal) { BT_PROFILE("setConstraints"); for (int i = 0; i < m_softBodies.size(); ++i) @@ -124,7 +124,7 @@ void btDeformableContactProjection::setConstraints() { if (psb->m_nodes[j].m_im == 0) { - btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]); + btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal); m_staticConstraints[i].push_back(static_constraint); } } @@ -139,7 +139,7 @@ void btDeformableContactProjection::setConstraints() continue; } anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; - btDeformableNodeAnchorConstraint constraint(anchor); + btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal); m_nodeAnchorConstraints[i].push_back(constraint); } @@ -152,7 +152,7 @@ void btDeformableContactProjection::setConstraints() { continue; } - btDeformableNodeRigidContactConstraint constraint(contact); + btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); btVector3 va = constraint.getVa(); btVector3 vb = constraint.getVb(); const btVector3 vr = vb - va; @@ -173,7 +173,7 @@ void btDeformableContactProjection::setConstraints() { continue; } - btDeformableFaceRigidContactConstraint constraint(contact); + btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal); btVector3 va = constraint.getVa(); btVector3 vb = constraint.getVb(); const btVector3 vr = vb - va; @@ -190,7 +190,7 @@ void btDeformableContactProjection::setConstraints() { const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; - btDeformableFaceNodeContactConstraint constraint(contact); + btDeformableFaceNodeContactConstraint constraint(contact, infoGlobal); btVector3 va = constraint.getVa(); btVector3 vb = constraint.getVb(); const btVector3 vr = vb - va; diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 3c4490765..32d6c4d75 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -72,13 +72,13 @@ public: virtual void applyDynamicFriction(TVStack& f); // update and solve the constraints - virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies); + virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); // solve the position error using split impulse virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. - virtual void setConstraints(); + virtual void setConstraints(const btContactSolverInfo& infoGlobal); // Set up projections for each vertex by adding the projection direction to virtual void setProjection(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 06f95d69f..c8cc47923 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -32,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); // solver body velocity -> rigid body velocity solverBodyWriteBack(infoGlobal); - btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies); + btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal); // update rigid body velocity in rigid/deformable contact m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); // solver body velocity <- rigid body velocity @@ -112,7 +112,7 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI if (infoGlobal.m_splitImpulse) { { - m_deformableSolver->splitImpulseSetup(infoGlobal); +// m_deformableSolver->splitImpulseSetup(infoGlobal); for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { btScalar leastSquaresResidual = 0.f; @@ -127,8 +127,8 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } // solve the position correction between deformable and rigid/multibody - btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); - leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); +// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); +// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index af6f9806c..f8de4b11e 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -285,7 +285,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::setupConstraints() { // set up constraints between multibody and deformable bodies - m_deformableBodySolver->setConstraints(); + m_deformableBodySolver->setConstraints(m_solverInfo); // set up constraints among multibodies { diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 2a458b1d8..634778d00 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -53,6 +53,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV n.m_material = pm; } updateBounds(); + setCollisionQuadrature(3); } btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) @@ -2403,10 +2404,9 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect // but resolve contact at x_n -// btTransform wtr = (predict) ? -// (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) -// : colObjWrap->getWorldTransform(); - const btTransform& wtr = colObjWrap->getWorldTransform(); + btTransform wtr = (predict) ? + (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), @@ -2457,10 +2457,9 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) : colObjWrap->getWorldTransform(); -// const btTransform& wtr = colObjWrap->getWorldTransform(); btScalar dst; -//#define USE_QUADRATURE 1 +#define USE_QUADRATURE 1 //#define CACHE_PREV_COLLISION // use the contact position of the previous collision @@ -2476,6 +2475,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO nrm, margin); nrm = wtr.getBasis() * nrm; + cti.m_colObj = colObjWrap->getCollisionObject(); // use cached contact point } else @@ -2492,10 +2492,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO contact_point = results.witnesses[0]; getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); nrm = results.normal; + cti.m_colObj = colObjWrap->getCollisionObject(); for (int i = 0; i < 3; ++i) f.m_pcontact[i] = bary[i]; } - + return (dst < 0); #endif // use collision quadrature point @@ -2505,7 +2506,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btVector3 local_nrm; for (int q = 0; q < m_quads.size(); ++q) { - btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); + btVector3 p; + if (predict) + p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]); + else + p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(p), shp, @@ -2513,12 +2518,21 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO margin); if (local_dst < dst) { + if (local_dst < 0 && predict) + return true; dst = local_dst; contact_point = p; bary = m_quads[q]; - nrm = wtr.getBasis() * local_nrm; + nrm = local_nrm; + } + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = dst; } } + return (dst < 0); } #endif @@ -2530,6 +2544,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO triangle_transform.setOrigin(f.m_n[0]->m_x); btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); btVector3 guess(0,0,0); + if (predict) + { + triangle_transform.setOrigin(f.m_n[0]->m_q); + triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); + } const btConvexShape* csh = static_cast(shp); btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); dst = results.distance - margin; @@ -2547,9 +2566,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO cti.m_offset = dst; } - if (dst < 0) - return true; - return (false); + return (dst < 0); } // @@ -3681,8 +3698,8 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap docollideFace.psb = this; docollideFace.m_colObj1Wrap = pcoWrap; docollideFace.m_rigidBody = prb1; - docollideFace.dynmargin = basemargin + timemargin; - docollideFace.stamargin = basemargin; + docollideFace.dynmargin = 0.05*(basemargin + timemargin); + docollideFace.stamargin = 0.05*basemargin; m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); } } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index cde4746d5..6ef0ee367 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1070,8 +1070,8 @@ struct btSoftColliders if (!n.m_battach) { - // check for collision at x_{n+1}^* as well at x_n - if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) + // check for collision at x_{n+1}^* + if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) { const btScalar ima = n.m_im; // todo: collision between multibody and fixed deformable node will be missed. @@ -1159,7 +1159,6 @@ struct btSoftColliders btSoftBody::Node* n0 = f.m_n[0]; btSoftBody::Node* n1 = f.m_n[1]; btSoftBody::Node* n2 = f.m_n[2]; - const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin; btSoftBody::DeformableFaceRigidContact c; btVector3 contact_point; @@ -1181,6 +1180,8 @@ struct btSoftColliders // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary; c.m_face = &f; + // friction is handled by the nodes to prevent sticking +// const btScalar fc = 0; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf