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