AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
Erwin Coumans 2019-11-29 08:06:53 -08:00
commit cf67e7af0a
10 changed files with 264 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
examples/DeformableDemo/PinchFriction.cpp
View File

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

1
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -1642,7 +1642,6 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
#endif
btMultiBodyDynamicsWorld* m_dynamicsWorld;
int m_constraintSolverType;

1
examples/pybullet/examples/deformable_ball.py
View File

@ -13,6 +13,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
ballId = p.loadSoftBody("ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 0.1, useNeoHookean = 1, NeoHookeanMu = 20, NeoHookeanLambda = 20, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5)
p.setTimeStep(0.001)
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
p.setRealTimeSimulation(1)

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

126
src/BulletSoftBody/btSoftBody.cpp
View File

@ -117,7 +117,7 @@ void btSoftBody::initDefaults()
m_restLengthScale = btScalar(1.0);
m_dampingCoefficient = 1;
m_sleepingThreshold = 0.1;
m_useFaceContact = false;
m_useFaceContact = true;
m_useSelfCollision = false;
m_collisionFlags = 0;
}
@ -325,7 +325,7 @@ void btSoftBody::appendFace(int model, Material* mat)
ZeroInitialize(f);
f.m_material = mat ? mat : m_materials[0];
}
m_faces.push_back(f);
m_faces.push_back(f);
}
//
@ -2443,7 +2443,7 @@ static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVec
//
bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap,
const Face& f,
Face& f,
btVector3& contact_point,
btVector3& bary,
btScalar margin,
@ -2457,25 +2457,96 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
btTransform wtr = (predict) ?
(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
: colObjWrap->getWorldTransform();
// const btTransform& wtr = colObjWrap->getWorldTransform();
btScalar dst;
//#define USE_QUADRATURE 1
//#define CACHE_PREV_COLLISION
// use the contact position of the previous collision
#ifdef CACHE_PREV_COLLISION
if (f.m_pcontact[3] != 0)
{
for (int i = 0; i < 3; ++i)
bary[i] = f.m_pcontact[i];
contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
dst = m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(contact_point),
shp,
nrm,
margin);
nrm = wtr.getBasis() * nrm;
// use cached contact point
}
else
{
btGjkEpaSolver2::sResults results;
btTransform triangle_transform;
triangle_transform.setIdentity();
triangle_transform.setOrigin(f.m_n[0]->m_x);
btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
btVector3 guess(0,0,0);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
dst = results.distance - margin;
contact_point = results.witnesses[0];
getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
nrm = results.normal;
for (int i = 0; i < 3; ++i)
f.m_pcontact[i] = bary[i];
}
#endif
// use collision quadrature point
#ifdef USE_QUADRATURE
{
dst = SIMD_INFINITY;
btVector3 local_nrm;
for (int q = 0; q < m_quads.size(); ++q)
{
btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(p),
shp,
local_nrm,
margin);
if (local_dst < dst)
{
dst = local_dst;
contact_point = p;
bary = m_quads[q];
nrm = wtr.getBasis() * local_nrm;
}
}
}
#endif
// regular face contact
{
btGjkEpaSolver2::sResults results;
btTransform triangle_transform;
triangle_transform.setIdentity();
triangle_transform.setOrigin(f.m_n[0]->m_x);
btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
btVector3 guess(0,0,0);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
dst = results.distance - margin;
contact_point = results.witnesses[0];
getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
nrm = results.normal;
for (int i = 0; i < 3; ++i)
f.m_pcontact[i] = bary[i];
}
btGjkEpaSolver2::sResults results;
btTransform triangle_transform;
triangle_transform.setIdentity();
triangle_transform.setOrigin(f.m_n[0]->m_x);
btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
btVector3 guess(0,0,0);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
btScalar dst = results.distance - margin;
contact_point = results.witnesses[0];
getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
if (!predict)
{
cti.m_colObj = colObjWrap->getCollisionObject();
// cti.m_normal = wtr.getBasis() * results.normal;
cti.m_normal = results.normal;
cti.m_normal = nrm;
cti.m_offset = dst;
}
if (dst < 0)
return true;
return (false);
@ -3315,6 +3386,17 @@ void btSoftBody::interpolateRenderMesh()
}
}
void btSoftBody::setCollisionQuadrature(int N)
{
for (int i = 0; i <= N; ++i)
{
for (int j = 0; i+j <= N; ++j)
{
m_quads.push_back(btVector3(btScalar(i)/btScalar(N), btScalar(j)/btScalar(N), btScalar(N-i-j)/btScalar(N)));
}
}
}
//
void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti)
{
@ -3706,12 +3788,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,
@ -3727,6 +3817,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);

5
src/BulletSoftBody/btSoftBody.h
View File

@ -288,6 +288,7 @@ public:
btVector3 m_normal; // Normal
btScalar m_ra; // Rest area
btDbvtNode* m_leaf; // Leaf data
btVector4 m_pcontact; // barycentric weights of the persistent contact
int m_index;
};
/* Tetra */
@ -801,6 +802,7 @@ public:
btScalar m_sleepingThreshold;
btScalar m_maxSpeedSquared;
bool m_useFaceContact;
btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
@ -1120,7 +1122,7 @@ public:
void initializeFaceTree();
btVector3 evaluateCom() const;
bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, const Face& x, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& x, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
void updateNormals();
void updateBounds();
@ -1142,6 +1144,7 @@ public:
void applyForces();
void setMaxStress(btScalar maxStress);
void interpolateRenderMesh();
void setCollisionQuadrature(int N);
static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti);

1
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@ -1500,7 +1500,6 @@ void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector
bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6);
}
// Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
// If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)

163
src/BulletSoftBody/btSoftBodyInternals.h
View File

@ -1166,6 +1166,7 @@ struct btSoftColliders
btVector3 bary;
if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
{
f.m_pcontact[3] = 1;
btScalar ima = n0->m_im + n1->m_im + n2->m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
// todo: collision between multibody and fixed deformable face will be missed.
@ -1236,6 +1237,10 @@ struct btSoftColliders
psb->m_faceRigidContacts.push_back(c);
}
}
else
{
f.m_pcontact[3] = 0;
}
}
btSoftBody* psb;
const btCollisionObjectWrapper* m_colObj1Wrap;
@ -1243,6 +1248,7 @@ struct btSoftColliders
btScalar dynmargin;
btScalar stamargin;
};
//
// CollideVF_SS
//
@ -1299,6 +1305,7 @@ struct btSoftColliders
btScalar mrg;
};
//
// CollideVF_DD
//
@ -1309,37 +1316,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 +1361,7 @@ struct btSoftColliders
}
btSoftBody* psb[2];
btScalar mrg;
bool useFaceNormal;
};
//
@ -1371,37 +1377,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 +1431,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;
};
};

2
src/BulletSoftBody/btSparseSDF.h
View File

@ -348,7 +348,7 @@ struct btSparseSdf
{
struct btS
{
int x, y, z,w;
int x, y, z, w;
void* p;
};