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/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 6c410733b..b2e2b6040 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -3702,12 +3702,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, @@ -3723,6 +3731,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/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 93aa1d3c0..4a89c1662 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1243,6 +1243,7 @@ struct btSoftColliders btScalar dynmargin; btScalar stamargin; }; + // // CollideVF_SS // @@ -1299,6 +1300,7 @@ struct btSoftColliders btScalar mrg; }; + // // CollideVF_DD // @@ -1309,37 +1311,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 +1356,7 @@ struct btSoftColliders } btSoftBody* psb[2]; btScalar mrg; + bool useFaceNormal; }; // @@ -1371,37 +1372,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 +1426,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; }; };