diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index e82d1b139..9dd964706 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -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(1.0); 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/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index e8219dc50..96fd8b5ae 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -140,8 +140,8 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btS { 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 = c.m_cti.m_offset; } btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) @@ -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) { diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 2a458b1d8..25766516d 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,10 @@ 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(); +// const btTransform& wtr = colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), @@ -2460,7 +2461,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO // 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 +2477,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 +2494,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 +2508,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 +2520,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 +2546,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 +2568,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO cti.m_offset = dst; } - if (dst < 0) - return true; - return (false); + return (dst < 0); } // diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index cde4746d5..894e80f38 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,7 +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; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + // friction is handled by the nodes to prevent sticking + const btScalar fc = 0; // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im;