From cb46440e178cbff29195f29a61d55bf559f982e2 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 25 Sep 2019 14:48:52 -0700 Subject: [PATCH] add normal cone optimization for self-collision --- .../DeformableSelfCollision.cpp | 1 + examples/DeformableDemo/PinchFriction.cpp | 12 +- .../BroadphaseCollision/btDbvt.h | 111 +++++++++++++++- src/BulletSoftBody/btDeformableBodySolver.cpp | 1 + .../btDeformableMultiBodyDynamicsWorld.cpp | 1 + src/BulletSoftBody/btSoftBody.cpp | 70 +++++++++- src/BulletSoftBody/btSoftBodyInternals.h | 123 +++++++++++++++++- 7 files changed, 303 insertions(+), 16 deletions(-) diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index be09af17c..7fc8755e9 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -153,6 +153,7 @@ void DeformableSelfCollision::initPhysics() btVector3(-s, h, +4*s), btVector3(+s, h, +4*s), 10,40, +// 2,2, 0, true, 0.01); diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp index ed69ddfd8..72e864ac3 100644 --- a/examples/DeformableDemo/PinchFriction.cpp +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -274,8 +274,8 @@ void PinchFriction::initPhysics() psb->scale(btVector3(2, 2, 1)); psb->translate(btVector3(0, 2.1, 2.2)); - psb->getCollisionShape()->setMargin(0.03); - psb->setTotalMass(1); + psb->getCollisionShape()->setMargin(0.1); + 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; @@ -303,8 +303,8 @@ void PinchFriction::initPhysics() psb2->scale(btVector3(2, 2, 1)); psb2->translate(btVector3(0, 2.1, -2.2)); - psb2->getCollisionShape()->setMargin(0.03); - psb2->setTotalMass(1); + psb2->getCollisionShape()->setMargin(0.1); + 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; @@ -332,8 +332,8 @@ void PinchFriction::initPhysics() psb3->scale(btVector3(2, 2, 1)); psb3->translate(btVector3(0, 2.1, 0)); - psb3->getCollisionShape()->setMargin(0.03); - psb3->setTotalMass(1); + psb3->getCollisionShape()->setMargin(0.1); + 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; diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.h b/src/BulletCollision/BroadphaseCollision/btDbvt.h index a316dbf20..51dbf3e58 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -21,7 +21,7 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btAabbUtil2.h" - +#include // // Compile time configuration // @@ -131,6 +131,8 @@ subject to the following restrictions: /* btDbvtAabbMm */ struct btDbvtAabbMm { + DBVT_INLINE btDbvtAabbMm(){} + DBVT_INLINE btDbvtAabbMm(const btDbvtAabbMm& other): mi(other.mi), mx(other.mx){} DBVT_INLINE btVector3 Center() const { return ((mi + mx) / 2); } DBVT_INLINE btVector3 Lengths() const { return (mx - mi); } DBVT_INLINE btVector3 Extents() const { return ((mx - mi) / 2); } @@ -190,6 +192,36 @@ struct btDbvtNode }; }; +/* btDbv(normal)tNode */ +struct btDbvntNode +{ + btDbvtVolume volume; + btVector3 normal; + btScalar angle; + DBVT_INLINE bool isleaf() const { return (childs[1] == 0); } + DBVT_INLINE bool isinternal() const { return (!isleaf()); } + btDbvntNode* childs[2]; + void* data; + + btDbvntNode(const btDbvtNode* n) + : volume(n->volume) + , angle(0) + , normal(0,0,0) + , data(n->data) + { + childs[0] = 0; + childs[1] = 0; + } + + ~btDbvntNode() + { + if (childs[0]) + delete childs[0]; + if (childs[1]) + delete childs[1]; + } +}; + typedef btAlignedObjectArray btNodeStack; ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). @@ -225,6 +257,14 @@ struct btDbvt btDbvtNode* parent; sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {} }; + + struct sStknNN + { + const btDbvntNode* a; + const btDbvntNode* b; + sStknNN() {} + sStknNN(const btDbvntNode* na, const btDbvntNode* nb) : a(na), b(nb) {} + }; // Policies/Interfaces /* ICollide */ @@ -234,6 +274,7 @@ struct btDbvt DBVT_VIRTUAL void Process(const btDbvtNode*, const btDbvtNode*) {} DBVT_VIRTUAL void Process(const btDbvtNode*) {} DBVT_VIRTUAL void Process(const btDbvtNode* n, btScalar) { Process(n); } + DBVT_VIRTUAL void Process(const btDbvntNode*, const btDbvntNode*) {} DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); } DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); } }; @@ -306,6 +347,9 @@ struct btDbvt void collideTT(const btDbvtNode* root0, const btDbvtNode* root1, DBVT_IPOLICY); + DBVT_PREFIX + void selfCollideT(const btDbvntNode* root, + DBVT_IPOLICY); DBVT_PREFIX void collideTTpersistentStack(const btDbvtNode* root0, @@ -837,6 +881,71 @@ inline void btDbvt::collideTT(const btDbvtNode* root0, } } +// +DBVT_PREFIX +inline void btDbvt::selfCollideT(const btDbvntNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if (root) + { + int depth = 1; + int treshold = DOUBLE_STACKSIZE - 4; + btAlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0] = sStknNN(root, root); + do + { + sStknNN p = stkStack[--depth]; + if (depth > treshold) + { + stkStack.resize(stkStack.size() * 2); + treshold = stkStack.size() - 4; + } + if (p.a == p.b) + { + if (p.a->isinternal() && p.a->angle > SIMD_PI) + { + stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[0]); + stkStack[depth++] = sStknNN(p.a->childs[1], p.a->childs[1]); + stkStack[depth++] = sStknNN(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++] = sStknNN(p.a->childs[0], p.b->childs[0]); + stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[0]); + stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[1]); + stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[1]); + } + else + { + stkStack[depth++] = sStknNN(p.a->childs[0], p.b); + stkStack[depth++] = sStknNN(p.a->childs[1], p.b); + } + } + else + { + if (p.b->isinternal()) + { + stkStack[depth++] = sStknNN(p.a, p.b->childs[0]); + stkStack[depth++] = sStknNN(p.a, p.b->childs[1]); + } + else + { + policy.Process(p.a, p.b); + } + } + } + } while (depth); + } +} + + DBVT_PREFIX inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0, const btDbvtNode* root1, diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 2167cb8ad..4ea724d06 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -415,6 +415,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d void btDeformableBodySolver::updateSoftBodies() { + BT_PROFILE("updateSoftBodies"); for (int i = 0; i < m_softBodies.size(); i++) { btSoftBody *psb = (btSoftBody *)m_softBodies[i]; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index d761d42e9..595ed737c 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -77,6 +77,7 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() { + m_deformableBodySolver->updateSoftBodies(); for (int i = 0; i < m_softBodies.size(); i++) { btSoftBody* psb = (btSoftBody*)m_softBodies[i]; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index eadc56d7c..0da85ff43 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -18,6 +18,7 @@ subject to the following restrictions: #include "BulletSoftBody/btSoftBodySolvers.h" #include "btSoftBodyData.h" #include "LinearMath/btSerializer.h" +#include "LinearMath/btAlignedAllocator.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" @@ -3453,9 +3454,55 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap } } +static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n) +{ + if (n == 0) + return 0; + btDbvntNode* root = new btDbvntNode(n); + if (n->isinternal()) + { + btDbvntNode* c0 = copyToDbvnt(n->childs[0]); + root->childs[0] = c0; + btDbvntNode* c1 = copyToDbvnt(n->childs[1]); + root->childs[1] = c1; + } + return root; +} + +static inline void calculateNormalCone(btDbvntNode* root) +{ + if (!root) + return; + if (root->isleaf()) + { + const btSoftBody::Face* face = (btSoftBody::Face*)root->data; + root->normal = face->m_normal; + root->angle = 0; + } + else + { + btVector3 n0(0,0,0), n1(0,0,0); + btScalar a0 = 0, a1 = 0; + if (root->childs[0]) + { + calculateNormalCone(root->childs[0]); + n0 = root->childs[0]->normal; + a0 = root->childs[0]->angle; + } + if (root->childs[1]) + { + calculateNormalCone(root->childs[1]); + n1 = root->childs[1]->normal; + a1 = root->childs[1]->angle; + } + root->normal = (n0+n1).safeNormalize(); + root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5; + } +} // void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { + BT_PROFILE("Deformable Collision"); const int cf = m_cfg.collisions & psb->m_cfg.collisions; switch (cf & fCollision::SVSmask) { @@ -3495,7 +3542,6 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) break; case fCollision::VF_DD: { - // self-collision not supported yet if (this != psb) { btSoftColliders::CollideVF_DD docollide; @@ -3517,15 +3563,27 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) } else { - btSoftColliders::CollideVF_DD docollide; + btSoftColliders::CollideFF_DD docollide; docollide.mrg = getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb0 faces */ 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); + /* psb0 faces vs psb0 faces */ + btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root); + calculateNormalCone(root); + this->m_fdbvt.selfCollideT(root,docollide); + delete root; + +// btSoftColliders::CollideFF_DD docollide; +// /* common */ +// docollide.mrg = getCollisionShape()->getMargin() + +// psb->getCollisionShape()->getMargin(); +// /* psb0 nodes vs psb1 faces */ +// docollide.psb[0] = this; +// docollide.psb[1] = psb; +// docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_fdbvt.m_root, +// docollide.psb[1]->m_fdbvt.m_root, +// docollide); } } break; diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 9f4d2a197..c494da598 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1306,8 +1306,8 @@ struct btSoftColliders // struct CollideVF_DD : btDbvt::ICollide { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) + void Process(const btDbvntNode* lnode, + const btDbvntNode* lface) { btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; btSoftBody::Face* face = (btSoftBody::Face*)lface->data; @@ -1324,7 +1324,7 @@ struct btSoftColliders 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; + btScalar scale = 2; // enlarge the triangle to catch collision on the edge btVector3 u0 = vc + (v0-vc)*scale; btVector3 u1 = vc + (v1-vc)*scale; @@ -1359,6 +1359,123 @@ struct btSoftColliders btSoftBody* psb[2]; btScalar mrg; }; + + // + // CollideFF_DD + // + struct CollideFF_DD : btDbvt::ICollide + { + void Process(const btDbvntNode* lface1, + const btDbvntNode* 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 = btMax(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); + } + } + } + } + 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 = btMax(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; + }; }; #endif //_BT_SOFT_BODY_INTERNALS_H