From 42b5c93bad7077c8133603cd5f290b020efd6d42 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 12 Nov 2019 11:50:47 -0800 Subject: [PATCH] approximate face contact WIP --- src/BulletSoftBody/btSoftBody.cpp | 58 ++++++++++++++++-------- src/BulletSoftBody/btSoftBody.h | 3 +- src/BulletSoftBody/btSoftBodyInternals.h | 5 ++ 3 files changed, 46 insertions(+), 20 deletions(-) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index b2e2b6040..6bbe50e8a 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -117,7 +117,7 @@ void btSoftBody::initDefaults() m_restLengthScale = btScalar(1.0); m_dampingCoefficient = 1; m_sleepingThreshold = 0.1; - m_useFaceContact = false; + m_useFaceContact = true; m_useSelfCollision = false; m_collisionFlags = 0; } @@ -2443,7 +2443,7 @@ static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVec // bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, - const Face& f, + Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, @@ -2454,26 +2454,46 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO 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(); - - btGjkEpaSolver2::sResults results; - btTransform triangle_transform; - triangle_transform.setIdentity(); - 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); - const btConvexShape* csh = static_cast(shp); - btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - btScalar dst = results.distance - margin; - 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); +// btTransform wtr = (predict) ? +// (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) +// : colObjWrap->getWorldTransform(); + const btTransform& wtr = colObjWrap->getWorldTransform(); + btScalar dst; +// if (f.m_pcontact[3] != 0) + if (0) + { + for (int i = 0; i < 3; ++i) + bary[i] = f.m_pcontact[i]; + contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + dst = m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(contact_point), + shp, + nrm, + margin); + nrm = wtr.getBasis() * nrm; + // use cached contact point + } + else + { + btGjkEpaSolver2::sResults results; + btTransform triangle_transform; + triangle_transform.setIdentity(); + 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); + const btConvexShape* csh = static_cast(shp); + btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); + dst = results.distance - margin; + 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; + for (int i = 0; i < 3; ++i) + f.m_pcontact[i] = bary[i]; + } if (!predict) { cti.m_colObj = colObjWrap->getCollisionObject(); -// cti.m_normal = wtr.getBasis() * results.normal; - cti.m_normal = results.normal; + cti.m_normal = nrm; cti.m_offset = dst; } if (dst < 0) diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 38cc162d2..b35445e94 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -288,6 +288,7 @@ public: btVector3 m_normal; // Normal btScalar m_ra; // Rest area btDbvtNode* m_leaf; // Leaf data + btVector4 m_pcontact; // barycentric weights of the persistent contact int m_index; }; /* Tetra */ @@ -1120,7 +1121,7 @@ public: void initializeFaceTree(); btVector3 evaluateCom() const; bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; - bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, const Face& x, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& x, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; void updateNormals(); void updateBounds(); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 4a89c1662..cde4746d5 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1166,6 +1166,7 @@ struct btSoftColliders btVector3 bary; if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) { + f.m_pcontact[3] = 1; btScalar ima = n0->m_im + n1->m_im + n2->m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; // todo: collision between multibody and fixed deformable face will be missed. @@ -1236,6 +1237,10 @@ struct btSoftColliders psb->m_faceRigidContacts.push_back(c); } } + else + { + f.m_pcontact[3] = 0; + } } btSoftBody* psb; const btCollisionObjectWrapper* m_colObj1Wrap;