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

change collision detection between deformables to from continuous to discrete

Browse Source
This commit is contained in:
Xuchen Han
2019-11-11 13:58:03 -08:00
parent b25d806b14
commit 794614f269
5 changed files with 155 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

24
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

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

12
src/BulletSoftBody/btSoftBody.cpp
View File

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

158
src/BulletSoftBody/btSoftBodyInternals.h
View File

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