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

add support for deformable vs. deformable contact

Browse Source
This commit is contained in:
Xuchen Han
2019-09-16 16:03:36 -07:00
parent 403eb62dfa
commit 1bfb226be8
12 changed files with 426 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
examples/DeformableDemo/DeformableContact.cpp Normal file
View File

@@ -0,0 +1,8 @@
//
// DeformableContact.cpp
// App_BulletExampleBrowser
//
// Created by Xuchen Han on 9/16/19.
//
#include "DeformableContact.hpp"

13
examples/DeformableDemo/DeformableContact.h Normal file
View File

@@ -0,0 +1,13 @@
//
// DeformableContact.hpp
// App_BulletExampleBrowser
//
// Created by Xuchen Han on 9/16/19.
//
#ifndef DeformableContact_hpp
#define DeformableContact_hpp
#include <stdio.h>
#endif /* DeformableContact_hpp */

7
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@@ -71,13 +71,6 @@ public:
void setDt(btScalar dt);
// enforce constraints in contact solve
void enforceConstraint(TVStack& x)
{
BT_PROFILE("enforceConstraint");
projection.enforceConstraint(x);
}
void applyDynamicFriction(TVStack& r);
// add dv to velocity

2
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -409,7 +409,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
/* Clear contacts */
psb->m_nodeRigidContacts.resize(0);
psb->m_faceRigidContacts.resize(0);
psb->m_scontacts.resize(0);
psb->m_faceNodeContacts.resize(0);
/* Optimize dbvt's */
psb->m_ndbvt.optimizeIncremental(1);
psb->m_fdbvt.optimizeIncremental(1);

129
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@@ -196,14 +196,12 @@ btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node*
/* ================ Face vs. Rigid =================== */
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
: m_face(contact.m_face)
, m_solved(false)
, btDeformableRigidContactConstraint(contact)
{
}
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
: m_face(other.m_face)
, m_solved(false)
, btDeformableRigidContactConstraint(other)
{
}
@@ -231,3 +229,130 @@ btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node*
btAssert(node == m_face->m_n[2]);
return face_dv * contact->m_weights[2];
}
/* ================ Face vs. Node =================== */
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
: m_node(contact.m_node)
, m_face(contact.m_face)
, m_contact(&contact)
, btDeformableContactConstraint(contact.m_normal)
{
m_total_normal_dv.setZero();
m_total_tangent_dv.setZero();
}
btVector3 btDeformableFaceNodeContactConstraint::getVa() const
{
return m_node->m_v;
}
btVector3 btDeformableFaceNodeContactConstraint::getVb() const
{
const btSoftBody::DeformableFaceNodeContact* contact = getContact();
btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
return vb;
}
btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const
{
btVector3 dv = m_total_normal_dv + m_total_tangent_dv;
if (n == m_node)
return dv;
const btSoftBody::DeformableFaceNodeContact* contact = getContact();
if (m_face->m_n[0] == n)
{
return dv * contact->m_weights[0];
}
if (m_face->m_n[1] == n)
{
return dv * contact->m_weights[1];
}
btAssert(n == m_face->m_n[2]);
return dv * contact->m_weights[2];
}
btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
{
btVector3 va = getVa();
btVector3 vb = getVb();
btVector3 vr = vb - va;
const btScalar dn = btDot(vr, m_contact->m_normal);
// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
btScalar residualSquare = dn*dn;
btVector3 impulse = m_contact->m_c0 * vr;
const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
btVector3 old_total_tangent_dv = m_total_tangent_dv;
// m_c2 is the inverse mass of the deformable node/face
if (m_node->m_im > 0)
{
m_total_normal_dv -= impulse_normal * m_node->m_im;
m_total_tangent_dv -= impulse_tangent * m_node->m_im;
}
else
{
m_total_normal_dv -= impulse_normal * m_contact->m_imf;
m_total_tangent_dv -= impulse_tangent * m_contact->m_imf;
}
if (m_total_normal_dv.dot(m_contact->m_normal) < 0)
{
// separating in the normal direction
m_static = false;
m_total_tangent_dv = btVector3(0,0,0);
impulse_tangent.setZero();
}
else
{
if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm())
{
// dynamic friction
// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
m_static = false;
if (m_total_tangent_dv.norm() < SIMD_EPSILON)
{
m_total_tangent_dv = btVector3(0,0,0);
}
else
{
m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_friction;
}
impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
}
else
{
// static friction
m_static = true;
}
}
impulse = impulse_normal + impulse_tangent;
// apply impulse to deformable nodes involved and change their velocities
applyImpulse(impulse);
return residualSquare;
}
void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableFaceNodeContact* contact = getContact();
btVector3 dva = impulse * contact->m_node->m_im;
btVector3 dvb = impulse * contact->m_imf;
if (contact->m_node->m_im > 0)
{
contact->m_node->m_v += dva;
}
btSoftBody::Face* face = contact->m_face;
if (face->m_n[0]->m_im > 0)
{
face->m_n[0]->m_v -= dvb * contact->m_weights[0];
}
if (face->m_n[1]->m_im > 0)
{
face->m_n[1]->m_v -= dvb * contact->m_weights[1];
}
if (face->m_n[2]->m_im > 0)
{
face->m_n[2]->m_v -= dvb * contact->m_weights[2];
}
}

42
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@@ -59,6 +59,9 @@ public:
// get the velocity change of the soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const = 0;
// apply impulse to the soft body node and/or face involved
virtual void applyImpulse(const btVector3& impulse) = 0;
};
class btDeformableStaticConstraint : public btDeformableContactConstraint
@@ -100,6 +103,8 @@ public:
{
return btVector3(0,0,0);
}
virtual void applyImpulse(const btVector3& impulse){}
};
class btDeformableRigidContactConstraint : public btDeformableContactConstraint
@@ -120,8 +125,6 @@ public:
virtual btVector3 getVa() const;
virtual btScalar solveConstraint();
virtual void applyImpulse(const btVector3& impulse) = 0;
};
@@ -161,7 +164,6 @@ class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactCo
{
public:
const btSoftBody::Face* m_face;
bool m_solved;
btDeformableFaceRigidContactConstraint(){}
btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact);
btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
@@ -192,4 +194,38 @@ public:
face->m_n[2]->m_v -= dv * contact->m_weights[2];
}
};
class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint
{
public:
btSoftBody::Node* m_node;
btSoftBody::Face* m_face;
const btSoftBody::DeformableFaceNodeContact* m_contact;
btVector3 m_total_normal_dv;
btVector3 m_total_tangent_dv;
btDeformableFaceNodeContactConstraint(){}
btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact);
virtual ~btDeformableFaceNodeContactConstraint(){}
virtual btScalar solveConstraint();
// get the velocity of the object A in the contact
virtual btVector3 getVa() const;
// get the velocity of the object B in the contact
virtual btVector3 getVb() const;
// get the velocity change of the soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const;
const btSoftBody::DeformableFaceNodeContact* getContact() const
{
return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact);
}
virtual void applyImpulse(const btVector3& impulse);
};
#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

126
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -35,7 +35,7 @@ btScalar btDeformableContactProjection::update()
// face constraints
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
btDeformableFaceRigidContactConstraint* constraint = m_allFaceConstraints[index];
btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
btScalar localResidualSquare = constraint->solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
@@ -140,66 +140,64 @@ void btDeformableContactProjection::setConstraints()
delete constraint;
}
}
}
// todo xuchenhan@: set Deformable Face vs. Deformable Node
}
void btDeformableContactProjection::enforceConstraint(TVStack& x)
{
// x is set to zero when passed in
// loop through node constraints to add in contributions to dv
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints.getAtIndex(index);
// i is node index
int i = m_nodeRigidConstraints.getKeyAtIndex(index).getUid1();
int numConstraints = 1;
// int numConstraints = constraintsList.size();
// if (m_faceRigidConstraints.find(i) != NULL)
// {
// numConstraints += m_faceRigidConstraints[i]->size();
// }
for (int j = 0; j < constraintsList.size(); ++j)
// set Deformable Face vs. Deformable Node constraint
for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
{
const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
x[i] += constraint.getDv(constraint.getContact()->m_node)/btScalar(numConstraints);
}
}
// loop through face constraints to add in contributions to dv
// note that for each face constraint is owned by three nodes. Be careful here to only add the dv to the node that owns the constraint
for (int index = 0; index < m_faceRigidConstraints.size(); ++index)
{
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints.getAtIndex(index);
// i is node index
int i = m_faceRigidConstraints.getKeyAtIndex(index).getUid1();
for (int j = 0; j < constraintsList.size(); ++j)
{
const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j];
const btSoftBody::Face* face = constraint->m_face;
btSoftBody::Node* node;
// find the node that owns the constraint
for (int k = 0; k < 3; ++k)
const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
btDeformableFaceNodeContactConstraint* constraint = new btDeformableFaceNodeContactConstraint(contact);
btVector3 va = constraint->getVa();
btVector3 vb = constraint->getVb();
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, contact.m_normal);
if (dn > -SIMD_EPSILON)
{
if (face->m_n[k]->index == i)
btSoftBody::Node* node = contact.m_node;
btSoftBody::Face* face = contact.m_face;
m_allFaceConstraints.push_back(constraint);
if (node->m_im != 0)
{
node = face->m_n[k];
break;
if (m_deformableConstraints.find(node->index) == NULL)
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> constraintsList;
constraintsList.push_back(constraint);
m_deformableConstraints.insert(node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[node->index];
constraintsList.push_back(constraint);
}
}
// add face constraints to each of the nodes
for (int k = 0; k < 3; ++k)
{
btSoftBody::Node* node = face->m_n[k];
// static node does not need to own face/rigid constraint
if (node->m_im != 0)
{
if (m_deformableConstraints.find(node->index) == NULL)
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> constraintsList;
constraintsList.push_back(constraint);
m_deformableConstraints.insert(node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[node->index];
constraintsList.push_back(constraint);
}
}
}
}
x[i] += constraint->getDv(node);
else
{
delete constraint;
}
}
}
// todo xuchenhan@: add deformable deformable constraints' contribution to dv
// Finally, loop through static constraints to set dv of static nodes to zero
for (int index = 0; index < m_staticConstraints.size(); ++index)
{
const btDeformableStaticConstraint& constraint = *m_staticConstraints.getAtIndex(index);
int i = m_staticConstraints.getKeyAtIndex(index).getUid1();
x[i] = constraint.getDv(constraint.m_node);
}
}
void btDeformableContactProjection::project(TVStack& x)
@@ -258,7 +256,7 @@ void btDeformableContactProjection::setProjection()
hasConstraint = true;
}
// accumulate normals
// accumulate normals from Deformable Node vs. Rigid constraints
if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL)
{
hasConstraint = true;
@@ -276,6 +274,7 @@ void btDeformableContactProjection::setProjection()
}
}
// accumulate normals from Deformable Face vs. Rigid constraints
if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
{
hasConstraint = true;
@@ -293,6 +292,24 @@ void btDeformableContactProjection::setProjection()
}
}
// accumulate normals from Deformable Node vs. Deformable Face constraints
if (!existStaticConstraint && m_deformableConstraints.find(index) != NULL)
{
hasConstraint = true;
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[index];
for (int k = 0; k < constraintsList.size(); ++k)
{
if (constraintsList[k]->m_static)
{
existStaticConstraint = true;
break;
}
const btVector3& local_normal = constraintsList[k]->m_normal;
normals.push_back(local_normal);
averagedNormal += local_normal;
}
}
// build projections
if (!hasConstraint)
@@ -388,6 +405,7 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
m_staticConstraints.clear();
m_nodeRigidConstraints.clear();
m_faceRigidConstraints.clear();
m_deformableConstraints.clear();
m_projectionsDict.clear();
for (int i = 0; i < m_allFaceConstraints.size(); ++i)
{

9
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -31,7 +31,11 @@ public:
btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// map from node index to face rigid constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> m_allFaceConstraints;
// map from node index to deformable constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
// all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
// map from node index to projection directions
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
@@ -51,9 +55,6 @@ public:
// add to friction
virtual void applyDynamicFriction(TVStack& f);
// apply constraints to x in Ax=b
virtual void enforceConstraint(TVStack& x);
// update the constraints
virtual btScalar update();

3
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@@ -23,7 +23,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
m_maxOverrideNumSolverIterations = 150;
// m_maxOverrideNumSolverIterations = 150;
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
@@ -39,7 +39,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
#endif
m_analyticsData.m_numSolverCalls++;
std::cout << "Contact Residual = " << m_leastSquaresResidual << std::endl;
m_analyticsData.m_numIterationsUsed = iteration+1;
m_analyticsData.m_islandId = -2;
if (numBodies>0)

28
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -89,6 +89,7 @@ void btSoftBody::initDefaults()
m_cfg.diterations = 0;
m_cfg.citerations = 4;
m_cfg.collisions = fCollision::Default;
m_cfg.collisions |= fCollision::VF_DD;
m_pose.m_bvolume = false;
m_pose.m_bframe = false;
m_pose.m_volume = 0;
@@ -1861,8 +1862,7 @@ void btSoftBody::predictMotion(btScalar dt)
}
}
/* Clear contacts */
m_nodeRigidContacts.resize(0);
m_faceRigidContacts.resize(0);
m_rcontacts.resize(0);
m_scontacts.resize(0);
/* Optimize dbvt's */
m_ndbvt.optimizeIncremental(1);
@@ -3484,6 +3484,30 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
}
}
break;
case fCollision::VF_DD:
{
// self-collision not supported yet
if (this != psb)
{
btSoftColliders::CollideVF_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_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
/* psb1 nodes vs psb0 faces */
docollide.psb[0] = psb;
docollide.psb[1] = this;
docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
}
}
break;
default:
{
}

17
src/BulletSoftBody/btSoftBody.h
View File

@@ -163,10 +163,11 @@ public:
SDF_RD = 0x0003, ///DF based rigid vs deformable
SDF_RDF = 0x0004, ///DF based rigid vs deformable faces
SVSmask = 0x0030, ///Rigid versus soft mask
SVSmask = 0x00F0, ///Rigid versus soft mask
VF_SS = 0x0010, ///Vertex vs face soft vs soft handling
CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling
CL_SELF = 0x0040, ///Cluster soft body self collision
VF_DD = 0x0050, ///Vertex vs face soft vs soft handling
/* presets */
Default = SDF_RS,
END
@@ -362,6 +363,19 @@ public:
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
};
struct DeformableFaceNodeContact
{
Node* m_node; // Node
Face* m_face; // Face
btVector3 m_bary; // Barycentric weights
btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
btVector3 m_normal; // Normal
btScalar m_margin; // Margin
btScalar m_friction; // Friction
btScalar m_imf; // inverse mass of the face at contact point
btScalar m_c0; // scale of the impulse matrix;
};
/* SContact */
struct SContact
{
@@ -759,6 +773,7 @@ public:
tAnchorArray m_anchors; // Anchors
tRContactArray m_rcontacts; // Rigid contacts
btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
tSContactArray m_scontacts; // Soft contacts
tJointArray m_joints; // Joints

118
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -501,6 +501,77 @@ static inline void ProjectOrigin(const btVector3& a,
}
}
//
static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t)
{
btScalar a, f, u, v;
btVector3 e1 = v1 - v0;
btVector3 e2 = v2 - v0;
btVector3 h = dir.cross(e2);
a = e1.dot(h);
if (a > -0.00001 && a < 0.00001)
return (false);
f = btScalar(1) / a;
btVector3 s = origin - v0;
u = f * s.dot(h);
if (u < 0.0 || u > 1.0)
return (false);
btVector3 q = s.cross(e1);
v = f * dir.dot(q);
if (v < 0.0 || u + v > 1.0)
return (false);
// at this stage we can compute t to find out where
// the intersection point is on the line
t = f * e2.dot(q);
if (t > 0) // ray intersection
return (true);
else // this means that there is a line intersection
// but not a ray intersection
return (false);
}
static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal)
{
btVector3 dir = rayEnd - rayStart;
btScalar dir_norm = dir.norm();
if (dir_norm < SIMD_EPSILON)
return false;
dir.normalize();
btScalar t;
bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
if (ret)
{
if (t <= dir_norm)
{
sect = rayStart + dir * t;
}
else
{
ret = false;
}
}
if (ret)
{
btVector3 n = (p3-p1).cross(p2-p1);
n.safeNormalize();
if (n.dot(dir) < 0)
normal = n;
else
normal = -n;
}
return ret;
}
//
template <typename T>
static inline T BaryEval(const T& a,
@@ -1217,6 +1288,53 @@ struct btSoftColliders
btSoftBody* psb[2];
btScalar mrg;
};
//
// CollideVF_DD
//
struct CollideVF_DD : btDbvt::ICollide
{
void Process(const btDbvtNode* lnode,
const btDbvtNode* lface)
{
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);
bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, face->m_n[0]->m_x-o, face->m_n[1]->m_x-o, face->m_n[2]->m_x-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;
c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
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