From 2edd94c7228081668808ad53f86a5d07641c83c3 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 12 Nov 2019 17:10:39 -0800 Subject: [PATCH] add option to approximate face contact with collision quadrature points --- .../PhysicsServerCommandProcessor.cpp | 2 + src/BulletSoftBody/btSoftBody.cpp | 76 +++++++++++++++++-- src/BulletSoftBody/btSoftBody.h | 2 + 3 files changed, 73 insertions(+), 7 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4558a421a..96766b861 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8129,6 +8129,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar { psb->m_renderNodes.resize(0); } + // This value should really be a exposed parameter + psb->setCollisionQuadrature(5); btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity); m_data->m_dynamicsWorld->addForce(psb, gravityForce); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 6bbe50e8a..f89784ed4 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -325,7 +325,7 @@ void btSoftBody::appendFace(int model, Material* mat) ZeroInitialize(f); f.m_material = mat ? mat : m_materials[0]; } - m_faces.push_back(f); + m_faces.push_back(f); } // @@ -2454,13 +2454,18 @@ 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(); - 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; -// if (f.m_pcontact[3] != 0) - if (0) + +#define USE_QUADRATURE 1 +//#define CACHE_PREV_COLLISION + + // use the contact position of the previous collision +#ifdef CACHE_PREV_COLLISION + if (f.m_pcontact[3] != 0) { for (int i = 0; i < 3; ++i) bary[i] = f.m_pcontact[i]; @@ -2490,12 +2495,58 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO for (int i = 0; i < 3; ++i) f.m_pcontact[i] = bary[i]; } + +#endif + + // use collision quadrature point +#ifdef USE_QUADRATURE + { + dst = SIMD_INFINITY; + 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]); + btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(p), + shp, + local_nrm, + margin); + if (local_dst < dst) + { + dst = local_dst; + contact_point = p; + bary = m_quads[q]; + nrm = wtr.getBasis() * local_nrm; + } + } + } +#endif + + // regular face contact + { + 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 = nrm; cti.m_offset = dst; } + if (dst < 0) return true; return (false); @@ -3331,6 +3382,17 @@ void btSoftBody::interpolateRenderMesh() } } +void btSoftBody::setCollisionQuadrature(int N) +{ + for (int i = 0; i <= N; ++i) + { + for (int j = 0; i+j <= N; ++j) + { + m_quads.push_back(btVector3(btScalar(i)/btScalar(N), btScalar(j)/btScalar(N), btScalar(N-i-j)/btScalar(N))); + } + } +} + // void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index b35445e94..c3262f02c 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -802,6 +802,7 @@ public: btScalar m_sleepingThreshold; btScalar m_maxSpeedSquared; bool m_useFaceContact; + btAlignedObjectArray m_quads; // quadrature points for collision detection btAlignedObjectArray m_renderNodesInterpolationWeights; btAlignedObjectArray > m_renderNodesParents; @@ -1143,6 +1144,7 @@ public: void applyForces(); void setMaxStress(btScalar maxStress); void interpolateRenderMesh(); + void setCollisionQuadrature(int N); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti);