diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp index bf0d57d0b..657c36e46 100644 --- a/examples/DeformableDemo/PinchFriction.cpp +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -93,16 +93,6 @@ public: virtual void renderScene() { CommonRigidBodyBase::renderScene(); -// btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); -// -// for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) -// { -// btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; -// { -// btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); -// btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); -// } -// } } }; @@ -115,8 +105,8 @@ void dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world) btScalar pressTime = 0.45; btScalar liftTime = 5; btScalar shiftTime = 1.75; - btScalar holdTime = 4.5*1000; - btScalar dropTime = 5.3*1000; + btScalar holdTime = 7.5; + btScalar dropTime = 8.3; btTransform rbTransform; rbTransform.setIdentity(); btVector3 translation; @@ -259,7 +249,7 @@ void PinchFriction::initPhysics() btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); - body->setFriction(0); + body->setFriction(0.5); //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body); @@ -275,11 +265,11 @@ void PinchFriction::initPhysics() psb->scale(btVector3(2, 2, 1)); psb->translate(btVector3(0, 2.1, 2.2)); - psb->getCollisionShape()->setMargin(0.1); + psb->getCollisionShape()->setMargin(0.05); psb->setTotalMass(.6); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 20; + psb->m_cfg.kDF = 2; btSoftBodyHelpers::generateBoundaryFaces(psb); psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; @@ -304,11 +294,11 @@ void PinchFriction::initPhysics() psb2->scale(btVector3(2, 2, 1)); psb2->translate(btVector3(0, 2.1, -2.2)); - psb2->getCollisionShape()->setMargin(0.1); + psb2->getCollisionShape()->setMargin(0.05); psb2->setTotalMass(.6); psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb2->m_cfg.kCHR = 1; // collision hardness with rigid body - psb2->m_cfg.kDF = 20; + psb2->m_cfg.kDF = 2; psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; btSoftBodyHelpers::generateBoundaryFaces(psb2); @@ -333,11 +323,11 @@ void PinchFriction::initPhysics() psb3->scale(btVector3(2, 2, 1)); psb3->translate(btVector3(0, 2.1, 0)); - psb3->getCollisionShape()->setMargin(0.1); + psb3->getCollisionShape()->setMargin(0.05); psb3->setTotalMass(.6); psb3->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb3->m_cfg.kCHR = 1; // collision hardness with rigid body - psb3->m_cfg.kDF = 20; + psb3->m_cfg.kDF = 2; psb3->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb3->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; btSoftBodyHelpers::generateBoundaryFaces(psb3); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d3565c4d5..94159893e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1642,7 +1642,6 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_lf; #endif - btMultiBodyDynamicsWorld* m_dynamicsWorld; int m_constraintSolverType; diff --git a/examples/pybullet/examples/deformable_ball.py b/examples/pybullet/examples/deformable_ball.py index 4ff81b613..279737fa4 100644 --- a/examples/pybullet/examples/deformable_ball.py +++ b/examples/pybullet/examples/deformable_ball.py @@ -13,6 +13,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn) boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) ballId = p.loadSoftBody("ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 0.1, useNeoHookean = 1, NeoHookeanMu = 20, NeoHookeanLambda = 20, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5) +p.setTimeStep(0.001) p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) p.setRealTimeSimulation(1) diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.h b/src/BulletCollision/BroadphaseCollision/btDbvt.h index 324c499e2..e5db99722 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -349,6 +349,9 @@ struct btDbvt DBVT_PREFIX void selfCollideT(const btDbvntNode* root, DBVT_IPOLICY); + DBVT_PREFIX + void selfCollideTT(const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX void collideTTpersistentStack(const btDbvtNode* root0, @@ -944,6 +947,70 @@ inline void btDbvt::selfCollideT(const btDbvntNode* root, } } +// +DBVT_PREFIX +inline void btDbvt::selfCollideTT(const btDbvtNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if (root) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + btAlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0] = sStkNN(root, root); + do + { + sStkNN p = stkStack[--depth]; + if (depth > treshold) + { + stkStack.resize(stkStack.size() * 2); + treshold = stkStack.size() - 4; + } + if (p.a == p.b) + { + if (p.a->isinternal()) + { + stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]); + stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]); + } + } + else if (Intersect(p.a->volume, p.b->volume)) + { + if (p.a->isinternal()) + { + if (p.b->isinternal()) + { + stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]); + } + else + { + stkStack[depth++] = sStkNN(p.a->childs[0], p.b); + stkStack[depth++] = sStkNN(p.a->childs[1], p.b); + } + } + else + { + if (p.b->isinternal()) + { + stkStack[depth++] = sStkNN(p.a, p.b->childs[0]); + stkStack[depth++] = sStkNN(p.a, p.b->childs[1]); + } + else + { + policy.Process(p.a, p.b); + } + } + } + } while (depth); + } +} + DBVT_PREFIX inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0, diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 6cca9d887..82f343104 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -431,11 +431,10 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) { btSoftBody::Node& n = psb->m_nodes[i]; - vol = btDbvtVolume::FromCR(n.m_q, psb->m_sst.radmrg); - psb->m_ndbvt.update(n.m_leaf, - vol, - n.m_v * psb->m_sst.velmrg, - psb->m_sst.updmrg); + btVector3 points[2] = {n.m_x, n.m_q}; + vol = btDbvtVolume::FromPoints(points, 2); + vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg)); + psb->m_ndbvt.update(n.m_leaf, vol); } if (!psb->m_fdbvt.empty()) @@ -443,15 +442,12 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d for (int i = 0; i < psb->m_faces.size(); ++i) { btSoftBody::Face& f = psb->m_faces[i]; - const btVector3 v = (f.m_n[0]->m_v + - f.m_n[1]->m_v + - f.m_n[2]->m_v) / - 3; - vol = VolumeOf(f, psb->m_sst.radmrg); - psb->m_fdbvt.update(f.m_leaf, - vol, - v * psb->m_sst.velmrg, - psb->m_sst.updmrg); + btVector3 points[6] = {f.m_n[0]->m_x, f.m_n[0]->m_q, + f.m_n[1]->m_x, f.m_n[1]->m_q, + f.m_n[2]->m_x, f.m_n[2]->m_q}; + vol = btDbvtVolume::FromPoints(points, 6); + vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg)); + psb->m_fdbvt.update(f.m_leaf, vol); } } /* Clear contacts */ diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index e4219b959..2a458b1d8 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; } @@ -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); } // @@ -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, @@ -2457,25 +2457,96 @@ 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 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]; + 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]; + } + +#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]; + } - 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); 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) return true; return (false); @@ -3315,6 +3386,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) { @@ -3706,12 +3788,20 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) docollide.mrg = getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin(); /* psb0 nodes vs psb1 faces */ + if (psb->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; docollide.psb[0] = this; docollide.psb[1] = psb; docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, docollide.psb[1]->m_fdbvt.m_root, docollide); /* psb1 nodes vs psb0 faces */ + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; docollide.psb[0] = psb; docollide.psb[1] = this; docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, @@ -3727,6 +3817,10 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) psb->getCollisionShape()->getMargin(); docollide.psb[0] = this; docollide.psb[1] = psb; + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; /* psb0 faces vs psb0 faces */ btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root); calculateNormalCone(root); diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 38cc162d2..c3262f02c 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 */ @@ -801,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; @@ -1120,7 +1122,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(); @@ -1142,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); diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 27221b574..649d6f58c 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1500,7 +1500,6 @@ void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6); } - // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 93aa1d3c0..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; @@ -1243,6 +1248,7 @@ struct btSoftColliders btScalar dynmargin; btScalar stamargin; }; + // // CollideVF_SS // @@ -1299,6 +1305,7 @@ struct btSoftColliders btScalar mrg; }; + // // CollideVF_DD // @@ -1309,37 +1316,35 @@ struct btSoftColliders { btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 o = node->m_x; - btVector3 p, normal; - const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; - btVector3 dir = node->m_q - o; - btScalar l = dir.length(); - if (l < SIMD_EPSILON) - return; - btVector3 rayEnd = dir.normalized() * (l + 2*mrg); - // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face - btVector3 v0 = face->m_n[0]->m_x; - btVector3 v1 = face->m_n[1]->m_x; - btVector3 v2 = face->m_n[2]->m_x; - btVector3 vc = (v0+v1+v2)/3.; - btScalar scale = 1.5; - // enlarge the triangle to catch collision on the edge - btVector3 u0 = vc + (v0-vc)*scale; - btVector3 u1 = vc + (v1-vc)*scale; - btVector3 u2 = vc + (v2-vc)*scale; - bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal); - - if (intersect) + btVector3 p; + btScalar d = SIMD_INFINITY; + ProjectOrigin(face->m_n[0]->m_x - o, + face->m_n[1]->m_x - o, + face->m_n[2]->m_x - o, + p, d); + const btScalar m = mrg + (o - node->m_q).safeNorm() * 2; + if (d < (m * m)) { - p += o; - const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p); + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o); const btScalar ma = node->m_im; btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + if ((n[0]->m_im <= 0) || + (n[1]->m_im <= 0) || + (n[2]->m_im <= 0)) + { + mb = 0; + } const btScalar ms = ma + mb; if (ms > 0) { btSoftBody::DeformableFaceNodeContact c; - c.m_normal = normal; + if (useFaceNormal) + c.m_normal = face->m_normal; + else + c.m_normal = p / -btSqrt(d); c.m_margin = mrg; c.m_node = node; c.m_face = face; @@ -1356,6 +1361,7 @@ struct btSoftColliders } btSoftBody* psb[2]; btScalar mrg; + bool useFaceNormal; }; // @@ -1371,37 +1377,45 @@ struct btSoftColliders for (int node_id = 0; node_id < 3; ++node_id) { btSoftBody::Node* node = f->m_n[node_id]; - btVector3 o = node->m_x; - btVector3 p, normal; - const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; - btVector3 dir = node->m_q - o; - btScalar l = dir.length(); - if (l < SIMD_EPSILON) - return; - btVector3 rayEnd = dir.normalized() * (l + 2*mrg); - // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face - btVector3 v0 = face->m_n[0]->m_x; - btVector3 v1 = face->m_n[1]->m_x; - btVector3 v2 = face->m_n[2]->m_x; - btVector3 vc = (v0+v1+v2)/3.; - btScalar scale = 1.5; - // enlarge the triangle to catch collision on the edge - btVector3 u0 = vc + (v0-vc)*scale; - btVector3 u1 = vc + (v1-vc)*scale; - btVector3 u2 = vc + (v2-vc)*scale; - bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal); - - if (intersect) + bool skip = false; + for (int i = 0; i < 3; ++i) { - p += o; - const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p); + if (face->m_n[i] == node) + { + skip = true; + break; + } + } + if (skip) + continue; + btVector3 o = node->m_x; + btVector3 p; + btScalar d = SIMD_INFINITY; + ProjectOrigin(face->m_n[0]->m_x - o, + face->m_n[1]->m_x - o, + face->m_n[2]->m_x - o, + p, d); + const btScalar m = mrg + (o - node->m_q).safeNorm() * 2; + if (d < (m * m)) + { + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o); const btScalar ma = node->m_im; btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + if ((n[0]->m_im <= 0) || + (n[1]->m_im <= 0) || + (n[2]->m_im <= 0)) + { + mb = 0; + } const btScalar ms = ma + mb; if (ms > 0) { btSoftBody::DeformableFaceNodeContact c; - c.m_normal = normal; + if (useFaceNormal) + c.m_normal = face->m_normal; + else + c.m_normal = p / -btSqrt(d); c.m_margin = mrg; c.m_node = node; c.m_face = face; @@ -1417,62 +1431,9 @@ struct btSoftColliders } } } - void Process(const btDbvtNode* lface1, - const btDbvtNode* lface2) - { - btSoftBody::Face* f = (btSoftBody::Face*)lface1->data; - btSoftBody::Face* face = (btSoftBody::Face*)lface2->data; - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f->m_n[node_id]; - btVector3 o = node->m_x; - btVector3 p, normal; - const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; - btVector3 dir = node->m_q - o; - btScalar l = dir.length(); - if (l < SIMD_EPSILON) - return; - btVector3 rayEnd = dir.normalized() * (l + 2*mrg); - // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face - btVector3 v0 = face->m_n[0]->m_x; - btVector3 v1 = face->m_n[1]->m_x; - btVector3 v2 = face->m_n[2]->m_x; - btVector3 vc = (v0+v1+v2)/3.; - btScalar scale = 1.5; - // enlarge the triangle to catch collision on the edge - btVector3 u0 = vc + (v0-vc)*scale; - btVector3 u1 = vc + (v1-vc)*scale; - btVector3 u2 = vc + (v2-vc)*scale; - bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal); - - if (intersect) - { - p += o; - const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p); - const btScalar ma = node->m_im; - btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); - const btScalar ms = ma + mb; - if (ms > 0) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = normal; - c.m_margin = mrg; - c.m_node = node; - c.m_face = face; - c.m_bary = w; - // 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) + w.length2()) * w; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf - c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im; - c.m_c0 = btScalar(1)/(ma + c.m_imf); - psb[0]->m_faceNodeContacts.push_back(c); - } - } - } - } btSoftBody* psb[2]; btScalar mrg; + bool useFaceNormal; }; }; diff --git a/src/BulletSoftBody/btSparseSDF.h b/src/BulletSoftBody/btSparseSDF.h index 97b068caa..eb290a1db 100644 --- a/src/BulletSoftBody/btSparseSDF.h +++ b/src/BulletSoftBody/btSparseSDF.h @@ -348,7 +348,7 @@ struct btSparseSdf { struct btS { - int x, y, z,w; + int x, y, z, w; void* p; };