Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add normal cone optimization for self-collision

Browse Source
This commit is contained in:
Xuchen Han
2019-09-25 14:48:52 -07:00
parent 657a7468b3
commit cb46440e17
7 changed files with 303 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/DeformableDemo/DeformableSelfCollision.cpp
View File

@@ -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);

12
examples/DeformableDemo/PinchFriction.cpp
View File

@@ -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;

111
src/BulletCollision/BroadphaseCollision/btDbvt.h
View File

@@ -21,7 +21,7 @@ subject to the following restrictions:
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btAabbUtil2.h"
#include <iostream>
//
// 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<const btDbvtNode*> 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<sStknNN> 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,

1
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -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];

1
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@@ -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];

70
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -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;

123
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -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