summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h')
-rw-r--r--thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h2074
1 files changed, 1044 insertions, 1030 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h
index b9ebc95b6b..c17bbb5cd4 100644
--- a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h
+++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h
@@ -32,86 +32,85 @@ subject to the following restrictions:
// Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction
static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
- btMultiBodyJacobianData& jacobianData,
- const btVector3& contact_point,
- const btVector3& dir)
-{
- const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
- jacobianData.m_jacobians.resize(ndof);
- jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
- btScalar* jac = &jacobianData.m_jacobians[0];
-
- multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
- multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
+ btMultiBodyJacobianData& jacobianData,
+ const btVector3& contact_point,
+ const btVector3& dir)
+{
+ const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+ jacobianData.m_jacobians.resize(ndof);
+ jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
+ btScalar* jac = &jacobianData.m_jacobians[0];
+
+ multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
+ multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
}
static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3& u)
{
- btScalar ux = u.getX();
- btScalar uy = u.getY();
- btScalar uz = u.getZ();
- btScalar ax = std::abs(ux);
- btScalar ay = std::abs(uy);
- btScalar az = std::abs(uz);
- btVector3 v;
- if (ax <= ay && ax <= az)
- v = btVector3(0, -uz, uy);
- else if (ay <= ax && ay <= az)
- v = btVector3(-uz, 0, ux);
- else
- v = btVector3(-uy, ux, 0);
- v.normalize();
- return v;
+ btScalar ux = u.getX();
+ btScalar uy = u.getY();
+ btScalar uz = u.getZ();
+ btScalar ax = std::abs(ux);
+ btScalar ay = std::abs(uy);
+ btScalar az = std::abs(uz);
+ btVector3 v;
+ if (ax <= ay && ax <= az)
+ v = btVector3(0, -uz, uy);
+ else if (ay <= ax && ay <= az)
+ v = btVector3(-uz, 0, ux);
+ else
+ v = btVector3(-uy, ux, 0);
+ v.normalize();
+ return v;
}
static SIMD_FORCE_INLINE bool proximityTest(const btVector3& x1, const btVector3& x2, const btVector3& x3, const btVector3& x4, const btVector3& normal, const btScalar& mrg, btVector3& bary)
{
- btVector3 x43 = x4-x3;
- if (std::abs(x43.dot(normal)) > mrg)
- return false;
- btVector3 x13 = x1-x3;
- btVector3 x23 = x2-x3;
- btScalar a11 = x13.length2();
- btScalar a22 = x23.length2();
- btScalar a12 = x13.dot(x23);
- btScalar b1 = x13.dot(x43);
- btScalar b2 = x23.dot(x43);
- btScalar det = a11*a22 - a12*a12;
- if (det < SIMD_EPSILON)
- return false;
- btScalar w1 = (b1*a22-b2*a12)/det;
- btScalar w2 = (b2*a11-b1*a12)/det;
- btScalar w3 = 1-w1-w2;
- btScalar delta = mrg / std::sqrt(0.5*std::abs(x13.cross(x23).safeNorm()));
- bary = btVector3(w1,w2,w3);
- for (int i = 0; i < 3; ++i)
- {
- if (bary[i] < -delta || bary[i] > 1+delta)
- return false;
- }
- return true;
+ btVector3 x43 = x4 - x3;
+ if (std::abs(x43.dot(normal)) > mrg)
+ return false;
+ btVector3 x13 = x1 - x3;
+ btVector3 x23 = x2 - x3;
+ btScalar a11 = x13.length2();
+ btScalar a22 = x23.length2();
+ btScalar a12 = x13.dot(x23);
+ btScalar b1 = x13.dot(x43);
+ btScalar b2 = x23.dot(x43);
+ btScalar det = a11 * a22 - a12 * a12;
+ if (det < SIMD_EPSILON)
+ return false;
+ btScalar w1 = (b1 * a22 - b2 * a12) / det;
+ btScalar w2 = (b2 * a11 - b1 * a12) / det;
+ btScalar w3 = 1 - w1 - w2;
+ btScalar delta = mrg / std::sqrt(0.5 * std::abs(x13.cross(x23).safeNorm()));
+ bary = btVector3(w1, w2, w3);
+ for (int i = 0; i < 3; ++i)
+ {
+ if (bary[i] < -delta || bary[i] > 1 + delta)
+ return false;
+ }
+ return true;
}
static const int KDOP_COUNT = 13;
-static btVector3 dop[KDOP_COUNT]={btVector3(1,0,0),
- btVector3(0,1,0),
- btVector3(0,0,1),
- btVector3(1,1,0),
- btVector3(1,0,1),
- btVector3(0,1,1),
- btVector3(1,-1,0),
- btVector3(1,0,-1),
- btVector3(0,1,-1),
- btVector3(1,1,1),
- btVector3(1,-1,1),
- btVector3(1,1,-1),
- btVector3(1,-1,-1)
-};
+static btVector3 dop[KDOP_COUNT] = {btVector3(1, 0, 0),
+ btVector3(0, 1, 0),
+ btVector3(0, 0, 1),
+ btVector3(1, 1, 0),
+ btVector3(1, 0, 1),
+ btVector3(0, 1, 1),
+ btVector3(1, -1, 0),
+ btVector3(1, 0, -1),
+ btVector3(0, 1, -1),
+ btVector3(1, 1, 1),
+ btVector3(1, -1, 1),
+ btVector3(1, 1, -1),
+ btVector3(1, -1, -1)};
static inline int getSign(const btVector3& n, const btVector3& x)
{
btScalar d = n.dot(x);
- if (d>SIMD_EPSILON)
+ if (d > SIMD_EPSILON)
return 1;
- if (d<-SIMD_EPSILON)
+ if (d < -SIMD_EPSILON)
return -1;
return 0;
}
@@ -119,13 +118,12 @@ static inline int getSign(const btVector3& n, const btVector3& x)
static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
{
btVector3 hex[6] = {face->m_n[0]->m_x - node->m_x,
- face->m_n[1]->m_x - node->m_x,
- face->m_n[2]->m_x - node->m_x,
- face->m_n[0]->m_x + dt*face->m_n[0]->m_v - node->m_x,
- face->m_n[1]->m_x + dt*face->m_n[1]->m_v - node->m_x,
- face->m_n[2]->m_x + dt*face->m_n[2]->m_v - node->m_x
- };
- btVector3 segment = dt*node->m_v;
+ face->m_n[1]->m_x - node->m_x,
+ face->m_n[2]->m_x - node->m_x,
+ face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x,
+ face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x,
+ face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x};
+ btVector3 segment = dt * node->m_v;
for (int i = 0; i < KDOP_COUNT; ++i)
{
int s = getSign(dop[i], segment);
@@ -143,488 +141,494 @@ static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, c
static SIMD_FORCE_INLINE bool nearZero(const btScalar& a)
{
- return (a>-SAFE_EPSILON && a<SAFE_EPSILON);
+ return (a > -SAFE_EPSILON && a < SAFE_EPSILON);
}
static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b)
{
- return (nearZero(a) || nearZero(b) || (a>SAFE_EPSILON && b>SAFE_EPSILON) || (a<-SAFE_EPSILON && b<-SAFE_EPSILON));
+ return (nearZero(a) || nearZero(b) || (a > SAFE_EPSILON && b > SAFE_EPSILON) || (a < -SAFE_EPSILON && b < -SAFE_EPSILON));
}
static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b)
{
- return !sameSign(a, b);
-}
-inline btScalar evaluateBezier2(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &t, const btScalar &s)
-{
- btScalar s2 = s*s;
- btScalar t2 = t*t;
-
- return p0*s2+p1*btScalar(2.0)*s*t+p2*t2;
-}
-inline btScalar evaluateBezier(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &p3, const btScalar &t, const btScalar &s)
-{
- btScalar s2 = s*s;
- btScalar s3 = s2*s;
- btScalar t2 = t*t;
- btScalar t3 = t2*t;
-
- return p0*s3+p1*btScalar(3.0)*s2*t+p2*btScalar(3.0)*s*t2+p3*t3;
-}
-static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar &lt0, btScalar &lt1)
-{
- if (sameSign(t0, t1)) {
- lt0 = t0;
- lt1 = t0;
- return true;
- }
-
- if (type_c || diffSign(k0, k3)) {
- btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
- if (t0<-0)
- ft = -ft;
-
- if (sameSign(ft, k0)) {
- lt0 = t1;
- lt1 = t1;
- }
- else {
- lt0 = t0;
- lt1 = t0;
- }
- return true;
- }
-
- if (!type_c) {
- btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
- if (t0<-0)
- ft = -ft;
-
- if (diffSign(ft, k0)) {
- lt0 = t0;
- lt1 = t1;
- return true;
- }
-
- btScalar fk = evaluateBezier2(k1-k0, k2-k1, k3-k2, t0, -t1);
-
- if (sameSign(fk, k1-k0))
- lt0 = lt1 = t1;
- else
- lt0 = lt1 = t0;
-
- return true;
- }
- return false;
+ return !sameSign(a, b);
+}
+inline btScalar evaluateBezier2(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& t, const btScalar& s)
+{
+ btScalar s2 = s * s;
+ btScalar t2 = t * t;
+
+ return p0 * s2 + p1 * btScalar(2.0) * s * t + p2 * t2;
+}
+inline btScalar evaluateBezier(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& p3, const btScalar& t, const btScalar& s)
+{
+ btScalar s2 = s * s;
+ btScalar s3 = s2 * s;
+ btScalar t2 = t * t;
+ btScalar t3 = t2 * t;
+
+ return p0 * s3 + p1 * btScalar(3.0) * s2 * t + p2 * btScalar(3.0) * s * t2 + p3 * t3;
+}
+static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar& lt0, btScalar& lt1)
+{
+ if (sameSign(t0, t1))
+ {
+ lt0 = t0;
+ lt1 = t0;
+ return true;
+ }
+
+ if (type_c || diffSign(k0, k3))
+ {
+ btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
+ if (t0 < -0)
+ ft = -ft;
+
+ if (sameSign(ft, k0))
+ {
+ lt0 = t1;
+ lt1 = t1;
+ }
+ else
+ {
+ lt0 = t0;
+ lt1 = t0;
+ }
+ return true;
+ }
+
+ if (!type_c)
+ {
+ btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
+ if (t0 < -0)
+ ft = -ft;
+
+ if (diffSign(ft, k0))
+ {
+ lt0 = t0;
+ lt1 = t1;
+ return true;
+ }
+
+ btScalar fk = evaluateBezier2(k1 - k0, k2 - k1, k3 - k2, t0, -t1);
+
+ if (sameSign(fk, k1 - k0))
+ lt0 = lt1 = t1;
+ else
+ lt0 = lt1 = t0;
+
+ return true;
+ }
+ return false;
}
static SIMD_FORCE_INLINE void getBernsteinCoeff(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, btScalar& k0, btScalar& k1, btScalar& k2, btScalar& k3)
{
- const btVector3& n0 = face->m_n0;
- const btVector3& n1 = face->m_n1;
- btVector3 n_hat = n0 + n1 - face->m_vn;
- btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x;
- btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q;
- k0 = (p0ma0).dot(n0) * 3.0;
- k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0);
- k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1);
- k3 = (p1ma1).dot(n1) * 3.0;
+ const btVector3& n0 = face->m_n0;
+ const btVector3& n1 = face->m_n1;
+ btVector3 n_hat = n0 + n1 - face->m_vn;
+ btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x;
+ btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q;
+ k0 = (p0ma0).dot(n0) * 3.0;
+ k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0);
+ k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1);
+ k3 = (p1ma1).dot(n1) * 3.0;
}
static SIMD_FORCE_INLINE void polyDecomposition(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, btScalar& u0, btScalar& u1, btScalar& v0, btScalar& v1)
{
- btScalar denom = 4.0 * (j1-j2) * (j1-j0) + (j2-j0) * (j2-j0);
- u0 = (2.0*(j1-j2)*(3.0*k1-2.0*k0-k3) - (j0-j2)*(3.0*k2-2.0*k3-k0)) / denom;
- u1 = (2.0*(j1-j0)*(3.0*k2-2.0*k3-k0) - (j2-j0)*(3.0*k1-2.0*k0-k3)) / denom;
- v0 = k0-u0*j0;
- v1 = k3-u1*j2;
+ btScalar denom = 4.0 * (j1 - j2) * (j1 - j0) + (j2 - j0) * (j2 - j0);
+ u0 = (2.0 * (j1 - j2) * (3.0 * k1 - 2.0 * k0 - k3) - (j0 - j2) * (3.0 * k2 - 2.0 * k3 - k0)) / denom;
+ u1 = (2.0 * (j1 - j0) * (3.0 * k2 - 2.0 * k3 - k0) - (j2 - j0) * (3.0 * k1 - 2.0 * k0 - k3)) / denom;
+ v0 = k0 - u0 * j0;
+ v1 = k3 - u1 * j2;
}
static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3)
{
- btScalar u0, u1, v0, v1;
- btScalar j0 = 3.0*(k1-k0);
- btScalar j1 = 3.0*(k2-k1);
- btScalar j2 = 3.0*(k3-k2);
- polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
- if (sameSign(v0, v1))
- {
- btScalar Ypa = j0*(1.0-v0)*(1.0-v0) + 2.0*j1*v0*(1.0-v0) + j2*v0*v0; // Y'(v0)
- if (sameSign(Ypa, j0))
- {
- return (diffSign(k0,v1));
- }
- }
- return diffSign(k0,v0);
-}
-
-static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2)
-{
- const btVector3& a0 = a->m_x;
- const btVector3& b0 = b->m_x;
- const btVector3& c0 = c->m_x;
- const btVector3& va = a->m_v;
- const btVector3& vb = b->m_v;
- const btVector3& vc = c->m_v;
- const btVector3 a1 = a0 + dt*va;
- const btVector3 b1 = b0 + dt*vb;
- const btVector3 c1 = c0 + dt*vc;
- btVector3 n0 = (b0-a0).cross(c0-a0);
- btVector3 n1 = (b1-a1).cross(c1-a1);
- btVector3 n_hat = n0+n1 - dt*dt*(vb-va).cross(vc-va);
- const btVector3& p0 = p->m_x;
- const btVector3& vp = p->m_v;
- btVector3 p1 = p0 + dt*vp;
- btVector3 m0 = (b0-p0).cross(c0-p0);
- btVector3 m1 = (b1-p1).cross(c1-p1);
- btVector3 m_hat = m0+m1 - dt*dt*(vb-vp).cross(vc-vp);
- btScalar l0 = m0.dot(n0);
- btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0));
- btScalar l2 = btScalar(1)/btScalar(6)*(m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0));
- btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat));
- btScalar l4 = m1.dot(n1);
-
- btScalar k1p = 0.25 * k0 + 0.75 * k1;
- btScalar k2p = 0.5 * k1 + 0.5 * k2;
- btScalar k3p = 0.75 * k2 + 0.25 * k3;
-
- btScalar s0 = (l1 * k0 - l0 * k1p)*4.0;
- btScalar s1 = (l2 * k0 - l0 * k2p)*2.0;
- btScalar s2 = (l3 * k0 - l0 * k3p)*btScalar(4)/btScalar(3);
- btScalar s3 = l4 * k0 - l0 * k3;
-
- j0 = (s1*k0 - s0*k1) * 3.0;
- j1 = (s2*k0 - s0*k2) * 1.5;
- j2 = (s3*k0 - s0*k3);
+ btScalar u0, u1, v0, v1;
+ btScalar j0 = 3.0 * (k1 - k0);
+ btScalar j1 = 3.0 * (k2 - k1);
+ btScalar j2 = 3.0 * (k3 - k2);
+ polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+ if (sameSign(v0, v1))
+ {
+ btScalar Ypa = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * v0 * (1.0 - v0) + j2 * v0 * v0; // Y'(v0)
+ if (sameSign(Ypa, j0))
+ {
+ return (diffSign(k0, v1));
+ }
+ }
+ return diffSign(k0, v0);
+}
+
+static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2)
+{
+ const btVector3& a0 = a->m_x;
+ const btVector3& b0 = b->m_x;
+ const btVector3& c0 = c->m_x;
+ const btVector3& va = a->m_v;
+ const btVector3& vb = b->m_v;
+ const btVector3& vc = c->m_v;
+ const btVector3 a1 = a0 + dt * va;
+ const btVector3 b1 = b0 + dt * vb;
+ const btVector3 c1 = c0 + dt * vc;
+ btVector3 n0 = (b0 - a0).cross(c0 - a0);
+ btVector3 n1 = (b1 - a1).cross(c1 - a1);
+ btVector3 n_hat = n0 + n1 - dt * dt * (vb - va).cross(vc - va);
+ const btVector3& p0 = p->m_x;
+ const btVector3& vp = p->m_v;
+ btVector3 p1 = p0 + dt * vp;
+ btVector3 m0 = (b0 - p0).cross(c0 - p0);
+ btVector3 m1 = (b1 - p1).cross(c1 - p1);
+ btVector3 m_hat = m0 + m1 - dt * dt * (vb - vp).cross(vc - vp);
+ btScalar l0 = m0.dot(n0);
+ btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0));
+ btScalar l2 = btScalar(1) / btScalar(6) * (m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0));
+ btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat));
+ btScalar l4 = m1.dot(n1);
+
+ btScalar k1p = 0.25 * k0 + 0.75 * k1;
+ btScalar k2p = 0.5 * k1 + 0.5 * k2;
+ btScalar k3p = 0.75 * k2 + 0.25 * k3;
+
+ btScalar s0 = (l1 * k0 - l0 * k1p) * 4.0;
+ btScalar s1 = (l2 * k0 - l0 * k2p) * 2.0;
+ btScalar s2 = (l3 * k0 - l0 * k3p) * btScalar(4) / btScalar(3);
+ btScalar s3 = l4 * k0 - l0 * k3;
+
+ j0 = (s1 * k0 - s0 * k1) * 3.0;
+ j1 = (s2 * k0 - s0 * k2) * 1.5;
+ j2 = (s3 * k0 - s0 * k3);
}
static SIMD_FORCE_INLINE bool signDetermination1Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1)
{
- btScalar Yu0 = k0*(1.0-u0)*(1.0-u0)*(1.0-u0) + 3.0*k1*u0*(1.0-u0)*(1.0-u0) + 3.0*k2*u0*u0*(1.0-u0) + k3*u0*u0*u0; // Y(u0)
- btScalar Yv0 = k0*(1.0-v0)*(1.0-v0)*(1.0-v0) + 3.0*k1*v0*(1.0-v0)*(1.0-v0) + 3.0*k2*v0*v0*(1.0-v0) + k3*v0*v0*v0; // Y(v0)
+ btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0)
+ btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(v0)
- btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0;
- btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0;
- sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
- btScalar K = (sameSign(sign_Ytp,k0)) ? v1 : v0;
- return diffSign(L,K);
+ btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0;
+ btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0;
+ sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
+ btScalar K = (sameSign(sign_Ytp, k0)) ? v1 : v0;
+ return diffSign(L, K);
}
static SIMD_FORCE_INLINE bool signDetermination2Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1)
{
- btScalar Yu0 = k0*(1.0-u0)*(1.0-u0)*(1.0-u0) + 3.0*k1*u0*(1.0-u0)*(1.0-u0) + 3.0*k2*u0*u0*(1.0-u0) + k3*u0*u0*u0; // Y(u0)
- btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2;
- if (diffSign(sign_Ytp,k0))
- {
- L1 = u0;
- L2 = u1;
- }
- else
- {
- btScalar Yp_u0 = j0*(1.0-u0)*(1.0-u0) + 2.0*j1*(1.0-u0)*u0 + j2*u0*u0;
- if (sameSign(Yp_u0,j0))
- {
- L1 = u1;
- L2 = u1;
- }
- else
- {
- L1 = u0;
- L2 = u0;
- }
- }
- btScalar Yv0 = k0*(1.0-v0)*(1.0-v0)*(1.0-v0) + 3.0*k1*v0*(1.0-v0)*(1.0-v0) + 3.0*k2*v0*v0*(1.0-v0) + k3*v0*v0*v0; // Y(uv0)
- sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
- btScalar K1, K2;
- if (diffSign(sign_Ytp,k0))
- {
- K1 = v0;
- K2 = v1;
- }
- else
- {
- btScalar Yp_v0 = j0*(1.0-v0)*(1.0-v0) + 2.0*j1*(1.0-v0)*v0 + j2*v0*v0;
- if (sameSign(Yp_v0,j0))
- {
- K1 = v1;
- K2 = v1;
- }
- else
- {
- K1 = v0;
- K2 = v0;
- }
- }
- return (diffSign(K1, L1) || diffSign(L2, K2));
+ btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0)
+ btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2;
+ if (diffSign(sign_Ytp, k0))
+ {
+ L1 = u0;
+ L2 = u1;
+ }
+ else
+ {
+ btScalar Yp_u0 = j0 * (1.0 - u0) * (1.0 - u0) + 2.0 * j1 * (1.0 - u0) * u0 + j2 * u0 * u0;
+ if (sameSign(Yp_u0, j0))
+ {
+ L1 = u1;
+ L2 = u1;
+ }
+ else
+ {
+ L1 = u0;
+ L2 = u0;
+ }
+ }
+ btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(uv0)
+ sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
+ btScalar K1, K2;
+ if (diffSign(sign_Ytp, k0))
+ {
+ K1 = v0;
+ K2 = v1;
+ }
+ else
+ {
+ btScalar Yp_v0 = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * (1.0 - v0) * v0 + j2 * v0 * v0;
+ if (sameSign(Yp_v0, j0))
+ {
+ K1 = v1;
+ K2 = v1;
+ }
+ else
+ {
+ K1 = v0;
+ K2 = v0;
+ }
+ }
+ return (diffSign(K1, L1) || diffSign(L2, K2));
}
static SIMD_FORCE_INLINE bool signDetermination1(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
{
- btScalar j0, j1, j2, u0, u1, v0, v1;
- // p1
- getJs(k0,k1,k2,k3,face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
- if (nearZero(j0+j2-j1*2.0))
- {
- btScalar lt0, lt1;
- getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
- if (lt0 < -SAFE_EPSILON)
- return false;
- }
- else
- {
- polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
- if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1))
- return false;
- }
- // p2
- getJs(k0,k1,k2,k3,face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
- if (nearZero(j0+j2-j1*2.0))
- {
- btScalar lt0, lt1;
- getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
- if (lt0 < -SAFE_EPSILON)
- return false;
- }
- else
- {
- polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
- if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1))
- return false;
- }
- // p3
- getJs(k0,k1,k2,k3,face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
- if (nearZero(j0+j2-j1*2.0))
- {
- btScalar lt0, lt1;
- getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
- if (lt0 < -SAFE_EPSILON)
- return false;
- }
- else
- {
- polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
- if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1))
- return false;
- }
- return true;
+ btScalar j0, j1, j2, u0, u1, v0, v1;
+ // p1
+ getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
+ if (nearZero(j0 + j2 - j1 * 2.0))
+ {
+ btScalar lt0, lt1;
+ getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
+ if (lt0 < -SAFE_EPSILON)
+ return false;
+ }
+ else
+ {
+ polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+ if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
+ return false;
+ }
+ // p2
+ getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
+ if (nearZero(j0 + j2 - j1 * 2.0))
+ {
+ btScalar lt0, lt1;
+ getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
+ if (lt0 < -SAFE_EPSILON)
+ return false;
+ }
+ else
+ {
+ polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+ if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
+ return false;
+ }
+ // p3
+ getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
+ if (nearZero(j0 + j2 - j1 * 2.0))
+ {
+ btScalar lt0, lt1;
+ getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
+ if (lt0 < -SAFE_EPSILON)
+ return false;
+ }
+ else
+ {
+ polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+ if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
+ return false;
+ }
+ return true;
}
static SIMD_FORCE_INLINE bool signDetermination2(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
{
- btScalar j0, j1, j2, u0, u1, v0, v1;
- // p1
- getJs(k0,k1,k2,k3,face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
- if (nearZero(j0+j2-j1*2.0))
- {
- btScalar lt0, lt1;
- bool bt0 = true, bt1=true;
- getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
- if (lt0 < -SAFE_EPSILON)
- bt0 = false;
- if (lt1 < -SAFE_EPSILON)
- bt1 = false;
- if (!bt0 && !bt1)
- return false;
- }
- else
- {
- polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
- if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1))
- return false;
- }
- // p2
- getJs(k0,k1,k2,k3,face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
- if (nearZero(j0+j2-j1*2.0))
- {
- btScalar lt0, lt1;
- bool bt0=true, bt1=true;
- getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
- if (lt0 < -SAFE_EPSILON)
- bt0 = false;
- if (lt1 < -SAFE_EPSILON)
- bt1 = false;
- if (!bt0 && !bt1)
- return false;
- }
- else
- {
- polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
- if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1))
- return false;
- }
- // p3
- getJs(k0,k1,k2,k3,face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
- if (nearZero(j0+j2-j1*2.0))
- {
- btScalar lt0, lt1;
- bool bt0=true, bt1=true;
- getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
- if (lt0 < -SAFE_EPSILON)
- bt0 = false;
- if (lt1 < -SAFE_EPSILON)
- bt1 = false;
- if (!bt0 && !bt1)
- return false;
- }
- else
- {
- polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1);
- if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1))
- return false;
- }
- return true;
+ btScalar j0, j1, j2, u0, u1, v0, v1;
+ // p1
+ getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
+ if (nearZero(j0 + j2 - j1 * 2.0))
+ {
+ btScalar lt0, lt1;
+ bool bt0 = true, bt1 = true;
+ getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
+ if (lt0 < -SAFE_EPSILON)
+ bt0 = false;
+ if (lt1 < -SAFE_EPSILON)
+ bt1 = false;
+ if (!bt0 && !bt1)
+ return false;
+ }
+ else
+ {
+ polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+ if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
+ return false;
+ }
+ // p2
+ getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
+ if (nearZero(j0 + j2 - j1 * 2.0))
+ {
+ btScalar lt0, lt1;
+ bool bt0 = true, bt1 = true;
+ getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
+ if (lt0 < -SAFE_EPSILON)
+ bt0 = false;
+ if (lt1 < -SAFE_EPSILON)
+ bt1 = false;
+ if (!bt0 && !bt1)
+ return false;
+ }
+ else
+ {
+ polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+ if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
+ return false;
+ }
+ // p3
+ getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
+ if (nearZero(j0 + j2 - j1 * 2.0))
+ {
+ btScalar lt0, lt1;
+ bool bt0 = true, bt1 = true;
+ getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
+ if (lt0 < -SAFE_EPSILON)
+ bt0 = false;
+ if (lt1 < -SAFE_EPSILON)
+ bt1 = false;
+ if (!bt0 && !bt1)
+ return false;
+ }
+ else
+ {
+ polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
+ if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
+ return false;
+ }
+ return true;
}
static SIMD_FORCE_INLINE bool coplanarAndInsideTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
{
- // Coplanar test
- if (diffSign(k1-k0, k3-k2))
- {
- // Case b:
- if (sameSign(k0, k3) && !rootFindingLemma(k0,k1,k2,k3))
- return false;
- // inside test
- return signDetermination2(k0, k1, k2, k3, face, node, dt);
- }
- else
- {
- // Case c:
- if (sameSign(k0, k3))
- return false;
- // inside test
- return signDetermination1(k0, k1, k2, k3, face, node, dt);
- }
- return false;
+ // Coplanar test
+ if (diffSign(k1 - k0, k3 - k2))
+ {
+ // Case b:
+ if (sameSign(k0, k3) && !rootFindingLemma(k0, k1, k2, k3))
+ return false;
+ // inside test
+ return signDetermination2(k0, k1, k2, k3, face, node, dt);
+ }
+ else
+ {
+ // Case c:
+ if (sameSign(k0, k3))
+ return false;
+ // inside test
+ return signDetermination1(k0, k1, k2, k3, face, node, dt);
+ }
+ return false;
}
static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg)
{
- if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg)
- return true;
- if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg)
- return true;
- return false;
+ if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg)
+ return true;
+ if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg)
+ return true;
+ return false;
}
static SIMD_FORCE_INLINE bool bernsteinVFTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
{
- if (conservativeCulling(k0, k1, k2, k3, mrg))
- return false;
- return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
+ if (conservativeCulling(k0, k1, k2, k3, mrg))
+ return false;
+ return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
}
static SIMD_FORCE_INLINE void deCasteljau(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, btScalar& k10, btScalar& k20, btScalar& k30, btScalar& k21, btScalar& k12)
{
- k10 = k0*(1.0-t0) + k1*t0;
- btScalar k11 = k1*(1.0-t0) + k2*t0;
- k12 = k2*(1.0-t0) + k3*t0;
- k20 = k10*(1.0-t0) + k11*t0;
- k21 = k11*(1.0-t0) + k12*t0;
- k30 = k20*(1.0-t0) + k21*t0;
+ k10 = k0 * (1.0 - t0) + k1 * t0;
+ btScalar k11 = k1 * (1.0 - t0) + k2 * t0;
+ k12 = k2 * (1.0 - t0) + k3 * t0;
+ k20 = k10 * (1.0 - t0) + k11 * t0;
+ k21 = k11 * (1.0 - t0) + k12 * t0;
+ k30 = k20 * (1.0 - t0) + k21 * t0;
}
static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg)
{
- btScalar k0, k1, k2, k3;
- getBernsteinCoeff(face, node, dt, k0, k1, k2, k3);
- if (conservativeCulling(k0, k1, k2, k3, mrg))
- return false;
- return true;
- if (diffSign(k2-2.0*k1+k0, k3-2.0*k2+k1))
- {
- btScalar k10, k20, k30, k21, k12;
- btScalar t0 = (k2-2.0*k1+k0)/(k0-3.0*k1+3.0*k2-k3);
- deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12);
- return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt);
- }
- return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
+ btScalar k0, k1, k2, k3;
+ getBernsteinCoeff(face, node, dt, k0, k1, k2, k3);
+ if (conservativeCulling(k0, k1, k2, k3, mrg))
+ return false;
+ return true;
+ if (diffSign(k2 - 2.0 * k1 + k0, k3 - 2.0 * k2 + k1))
+ {
+ btScalar k10, k20, k30, k21, k12;
+ btScalar t0 = (k2 - 2.0 * k1 + k0) / (k0 - 3.0 * k1 + 3.0 * k2 - k3);
+ deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12);
+ return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt);
+ }
+ return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
}
static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary)
{
- if (hasSeparatingPlane(face, node, dt))
- return false;
- btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x;
- btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x;
- btVector3 x41 = node->m_x - face->m_n[0]->m_x;
- btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v;
- btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v;
- btVector3 v41 = node->m_v - face->m_n[0]->m_v;
- btVector3 a = x21.cross(x31);
- btVector3 b = x21.cross(v31) + v21.cross(x31);
- btVector3 c = v21.cross(v31);
- btVector3 d = x41;
- btVector3 e = v41;
- btScalar a0 = a.dot(d);
- btScalar a1 = a.dot(e) + b.dot(d);
- btScalar a2 = c.dot(d) + b.dot(e);
- btScalar a3 = c.dot(e);
- btScalar eps = SAFE_EPSILON;
- int num_roots = 0;
- btScalar roots[3];
- if (std::abs(a3) < eps)
- {
- // cubic term is zero
- if (std::abs(a2) < eps)
- {
- if (std::abs(a1) < eps)
- {
- if (std::abs(a0) < eps)
- {
- num_roots = 2;
- roots[0] = 0;
- roots[1] = dt;
- }
- }
- else
- {
- num_roots = 1;
- roots[0] = -a0/a1;
- }
- }
- else
- {
- num_roots = SolveP2(roots, a1/a2, a0/a2);
- }
- }
- else
- {
- num_roots = SolveP3(roots, a2/a3, a1/a3, a0/a3);
- }
-// std::sort(roots, roots+num_roots);
- if (num_roots > 1)
- {
- if (roots[0] > roots[1])
- btSwap(roots[0], roots[1]);
- }
- if (num_roots > 2)
- {
- if (roots[0] > roots[2])
- btSwap(roots[0], roots[2]);
- if (roots[1] > roots[2])
- btSwap(roots[1], roots[2]);
- }
- for (int r = 0; r < num_roots; ++r)
- {
- double root = roots[r];
- if (root <= 0)
- continue;
- if (root > dt + SIMD_EPSILON)
- return false;
- btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v;
- btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v;
- btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v;
- btVector3 x4 = node->m_x + root * node->m_v;
- btVector3 normal = (x2-x1).cross(x3-x1);
- normal.safeNormalize();
- if (proximityTest(x1, x2, x3, x4, normal, mrg, bary))
- return true;
- }
- return false;
+ if (hasSeparatingPlane(face, node, dt))
+ return false;
+ btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x;
+ btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x;
+ btVector3 x41 = node->m_x - face->m_n[0]->m_x;
+ btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v;
+ btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v;
+ btVector3 v41 = node->m_v - face->m_n[0]->m_v;
+ btVector3 a = x21.cross(x31);
+ btVector3 b = x21.cross(v31) + v21.cross(x31);
+ btVector3 c = v21.cross(v31);
+ btVector3 d = x41;
+ btVector3 e = v41;
+ btScalar a0 = a.dot(d);
+ btScalar a1 = a.dot(e) + b.dot(d);
+ btScalar a2 = c.dot(d) + b.dot(e);
+ btScalar a3 = c.dot(e);
+ btScalar eps = SAFE_EPSILON;
+ int num_roots = 0;
+ btScalar roots[3];
+ if (std::abs(a3) < eps)
+ {
+ // cubic term is zero
+ if (std::abs(a2) < eps)
+ {
+ if (std::abs(a1) < eps)
+ {
+ if (std::abs(a0) < eps)
+ {
+ num_roots = 2;
+ roots[0] = 0;
+ roots[1] = dt;
+ }
+ }
+ else
+ {
+ num_roots = 1;
+ roots[0] = -a0 / a1;
+ }
+ }
+ else
+ {
+ num_roots = SolveP2(roots, a1 / a2, a0 / a2);
+ }
+ }
+ else
+ {
+ num_roots = SolveP3(roots, a2 / a3, a1 / a3, a0 / a3);
+ }
+ // std::sort(roots, roots+num_roots);
+ if (num_roots > 1)
+ {
+ if (roots[0] > roots[1])
+ btSwap(roots[0], roots[1]);
+ }
+ if (num_roots > 2)
+ {
+ if (roots[0] > roots[2])
+ btSwap(roots[0], roots[2]);
+ if (roots[1] > roots[2])
+ btSwap(roots[1], roots[2]);
+ }
+ for (int r = 0; r < num_roots; ++r)
+ {
+ double root = roots[r];
+ if (root <= 0)
+ continue;
+ if (root > dt + SIMD_EPSILON)
+ return false;
+ btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v;
+ btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v;
+ btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v;
+ btVector3 x4 = node->m_x + root * node->m_v;
+ btVector3 normal = (x2 - x1).cross(x3 - x1);
+ normal.safeNormalize();
+ if (proximityTest(x1, x2, x3, x4, normal, mrg, bary))
+ return true;
+ }
+ return false;
}
static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary)
{
- if (!bernsteinVFTest(face, node, dt, mrg))
- return false;
- if (!continuousCollisionDetection(face, node, dt, 1e-6, bary))
- return false;
- return true;
+ if (!bernsteinVFTest(face, node, dt, mrg))
+ return false;
+ if (!continuousCollisionDetection(face, node, dt, 1e-6, bary))
+ return false;
+ return true;
}
//
@@ -902,62 +906,61 @@ static inline btMatrix3x3 Diagonal(btScalar x)
static inline btMatrix3x3 Diagonal(const btVector3& v)
{
- btMatrix3x3 m;
- m[0] = btVector3(v.getX(), 0, 0);
- m[1] = btVector3(0, v.getY(), 0);
- m[2] = btVector3(0, 0, v.getZ());
- return (m);
-}
-
-static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof)
-{
- btScalar result = 0;
- for (int i = 0; i < ndof; ++i)
- result += a[i] * b[i];
- return result;
-}
-
-static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3,
- const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
-{
- btMatrix3x3 m;
- btScalar a11 = Dot(v1,u1,ndof);
- btScalar a12 = Dot(v1,u2,ndof);
- btScalar a13 = Dot(v1,u3,ndof);
-
- btScalar a21 = Dot(v2,u1,ndof);
- btScalar a22 = Dot(v2,u2,ndof);
- btScalar a23 = Dot(v2,u3,ndof);
-
- btScalar a31 = Dot(v3,u1,ndof);
- btScalar a32 = Dot(v3,u2,ndof);
- btScalar a33 = Dot(v3,u3,ndof);
- m[0] = btVector3(a11, a12, a13);
- m[1] = btVector3(a21, a22, a23);
- m[2] = btVector3(a31, a32, a33);
- return (m);
-}
-
-static inline btMatrix3x3 OuterProduct(const btVector3& v1,const btVector3& v2)
-{
- btMatrix3x3 m;
- btScalar a11 = v1[0] * v2[0];
- btScalar a12 = v1[0] * v2[1];
- btScalar a13 = v1[0] * v2[2];
-
- btScalar a21 = v1[1] * v2[0];
- btScalar a22 = v1[1] * v2[1];
- btScalar a23 = v1[1] * v2[2];
-
- btScalar a31 = v1[2] * v2[0];
- btScalar a32 = v1[2] * v2[1];
- btScalar a33 = v1[2] * v2[2];
- m[0] = btVector3(a11, a12, a13);
- m[1] = btVector3(a21, a22, a23);
- m[2] = btVector3(a31, a32, a33);
- return (m);
+ btMatrix3x3 m;
+ m[0] = btVector3(v.getX(), 0, 0);
+ m[1] = btVector3(0, v.getY(), 0);
+ m[2] = btVector3(0, 0, v.getZ());
+ return (m);
+}
+
+static inline btScalar Dot(const btScalar* a, const btScalar* b, int ndof)
+{
+ btScalar result = 0;
+ for (int i = 0; i < ndof; ++i)
+ result += a[i] * b[i];
+ return result;
}
+static inline btMatrix3x3 OuterProduct(const btScalar* v1, const btScalar* v2, const btScalar* v3,
+ const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
+{
+ btMatrix3x3 m;
+ btScalar a11 = Dot(v1, u1, ndof);
+ btScalar a12 = Dot(v1, u2, ndof);
+ btScalar a13 = Dot(v1, u3, ndof);
+
+ btScalar a21 = Dot(v2, u1, ndof);
+ btScalar a22 = Dot(v2, u2, ndof);
+ btScalar a23 = Dot(v2, u3, ndof);
+
+ btScalar a31 = Dot(v3, u1, ndof);
+ btScalar a32 = Dot(v3, u2, ndof);
+ btScalar a33 = Dot(v3, u3, ndof);
+ m[0] = btVector3(a11, a12, a13);
+ m[1] = btVector3(a21, a22, a23);
+ m[2] = btVector3(a31, a32, a33);
+ return (m);
+}
+
+static inline btMatrix3x3 OuterProduct(const btVector3& v1, const btVector3& v2)
+{
+ btMatrix3x3 m;
+ btScalar a11 = v1[0] * v2[0];
+ btScalar a12 = v1[0] * v2[1];
+ btScalar a13 = v1[0] * v2[2];
+
+ btScalar a21 = v1[1] * v2[0];
+ btScalar a22 = v1[1] * v2[1];
+ btScalar a23 = v1[1] * v2[2];
+
+ btScalar a31 = v1[2] * v2[0];
+ btScalar a32 = v1[2] * v2[1];
+ btScalar a33 = v1[2] * v2[2];
+ m[0] = btVector3(a11, a12, a13);
+ m[1] = btVector3(a21, a22, a23);
+ m[2] = btVector3(a31, a32, a33);
+ return (m);
+}
//
static inline btMatrix3x3 Add(const btMatrix3x3& a,
@@ -1008,6 +1011,20 @@ static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
}
//
+static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
+ const btMatrix3x3& effective_mass_inv,
+ btScalar imb,
+ const btMatrix3x3& iwi,
+ const btVector3& r)
+{
+ return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse());
+ // btMatrix3x3 iimb = MassMatrix(imb, iwi, r);
+ // if (iimb.determinant() == 0)
+ // return effective_mass_inv.inverse();
+ // return effective_mass_inv.inverse() * Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse();
+}
+
+//
static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
{
@@ -1091,73 +1108,70 @@ 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);
+ 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;
-}
+ 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>
@@ -1586,57 +1600,57 @@ struct btSoftColliders
psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this);
}
};
- //
- // CollideSDF_RS
- //
- struct CollideSDF_RS : btDbvt::ICollide
- {
- void Process(const btDbvtNode* leaf)
- {
- btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
- DoNode(*node);
- }
- void DoNode(btSoftBody::Node& n) const
- {
- const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
- btSoftBody::RContact c;
-
- if ((!n.m_battach) &&
- psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
- {
- const btScalar ima = n.m_im;
- const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
- const btScalar ms = ima + imb;
- if (ms > 0)
- {
- const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
- static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
- const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
- const btVector3 ra = n.m_x - wtr.getOrigin();
- const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
- const btVector3 vb = n.m_x - n.m_q;
- const btVector3 vr = vb - va;
- const btScalar dn = btDot(vr, c.m_cti.m_normal);
- const btVector3 fv = vr - c.m_cti.m_normal * dn;
- const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
- c.m_node = &n;
- c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
- c.m_c1 = ra;
- c.m_c2 = ima * psb->m_sst.sdt;
- c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
- c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
- psb->m_rcontacts.push_back(c);
- if (m_rigidBody)
- m_rigidBody->activate();
- }
- }
- }
- btSoftBody* psb;
- const btCollisionObjectWrapper* m_colObj1Wrap;
- btRigidBody* m_rigidBody;
- btScalar dynmargin;
- btScalar stamargin;
- };
+ //
+ // CollideSDF_RS
+ //
+ struct CollideSDF_RS : btDbvt::ICollide
+ {
+ void Process(const btDbvtNode* leaf)
+ {
+ btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
+ DoNode(*node);
+ }
+ void DoNode(btSoftBody::Node& n) const
+ {
+ const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
+ btSoftBody::RContact c;
+
+ if ((!n.m_battach) &&
+ psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
+ {
+ const btScalar ima = n.m_im;
+ const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+ const btScalar ms = ima + imb;
+ if (ms > 0)
+ {
+ const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+ static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+ const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+ const btVector3 ra = n.m_x - wtr.getOrigin();
+ const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
+ const btVector3 vb = n.m_x - n.m_q;
+ const btVector3 vr = vb - va;
+ const btScalar dn = btDot(vr, c.m_cti.m_normal);
+ const btVector3 fv = vr - c.m_cti.m_normal * dn;
+ const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+ c.m_node = &n;
+ c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
+ c.m_c1 = ra;
+ c.m_c2 = ima * psb->m_sst.sdt;
+ c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
+ c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+ psb->m_rcontacts.push_back(c);
+ if (m_rigidBody)
+ m_rigidBody->activate();
+ }
+ }
+ }
+ btSoftBody* psb;
+ const btCollisionObjectWrapper* m_colObj1Wrap;
+ btRigidBody* m_rigidBody;
+ btScalar dynmargin;
+ btScalar stamargin;
+ };
//
// CollideSDF_RD
@@ -1654,72 +1668,74 @@ struct btSoftColliders
btSoftBody::DeformableNodeRigidContact c;
if (!n.m_battach)
- {
+ {
// check for collision at x_{n+1}^*
if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
- {
- const btScalar ima = n.m_im;
- // todo: collision between multibody and fixed deformable node will be missed.
- const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
- const btScalar ms = ima + imb;
- if (ms > 0)
- {
- // resolve contact at x_n
- psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false);
- btSoftBody::sCti& cti = c.m_cti;
- c.m_node = &n;
- const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
- c.m_c2 = ima;
- c.m_c3 = fc;
- c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
-
- if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
- {
- const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
- static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
- const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
- const btVector3 ra = n.m_x - wtr.getOrigin();
-
- c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
- c.m_c1 = ra;
- }
- else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
- {
- btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
- if (multibodyLinkCol)
- {
- btVector3 normal = cti.m_normal;
- btVector3 t1 = generateUnitOrthogonalVector(normal);
- btVector3 t2 = btCross(normal, t1);
- btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
- findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal);
- findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1);
- findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2);
-
- btScalar* J_n = &jacobianData_normal.m_jacobians[0];
- btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
- btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
-
- btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
- btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
- btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
-
- btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
- t1.getX(), t1.getY(), t1.getZ(),
- t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
- const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
- btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
- c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
- c.jacobianData_normal = jacobianData_normal;
- c.jacobianData_t1 = jacobianData_t1;
- c.jacobianData_t2 = jacobianData_t2;
- c.t1 = t1;
- c.t2 = t2;
- }
- }
- psb->m_nodeRigidContacts.push_back(c);
- }
- }
+ {
+ const btScalar ima = n.m_im;
+ // todo: collision between multibody and fixed deformable node will be missed.
+ const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+ const btScalar ms = ima + imb;
+ if (ms > 0)
+ {
+ // resolve contact at x_n
+ psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false);
+ btSoftBody::sCti& cti = c.m_cti;
+ c.m_node = &n;
+ const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+ c.m_c2 = ima;
+ c.m_c3 = fc;
+ c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+ c.m_c5 = n.m_effectiveMass_inv;
+
+ if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+ {
+ const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+ static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+ const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+ const btVector3 ra = n.m_x - wtr.getOrigin();
+
+ c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
+ // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+ c.m_c1 = ra;
+ }
+ else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+ {
+ btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+ if (multibodyLinkCol)
+ {
+ btVector3 normal = cti.m_normal;
+ btVector3 t1 = generateUnitOrthogonalVector(normal);
+ btVector3 t2 = btCross(normal, t1);
+ btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+ findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal);
+ findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1);
+ findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2);
+
+ btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+ btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+ btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+
+ btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+ btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+ btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+
+ btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+ t1.getX(), t1.getY(), t1.getZ(),
+ t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
+ const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+ btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+ c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
+ c.jacobianData_normal = jacobianData_normal;
+ c.jacobianData_t1 = jacobianData_t1;
+ c.jacobianData_t2 = jacobianData_t2;
+ c.t1 = t1;
+ c.t2 = t2;
+ }
+ }
+ psb->m_nodeRigidContacts.push_back(c);
+ }
+ }
}
}
btSoftBody* psb;
@@ -1728,112 +1744,111 @@ struct btSoftColliders
btScalar dynmargin;
btScalar stamargin;
};
-
- //
- // CollideSDF_RDF
- //
- struct CollideSDF_RDF : btDbvt::ICollide
- {
- void Process(const btDbvtNode* leaf)
- {
- btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
- DoNode(*face);
- }
- void DoNode(btSoftBody::Face& f) const
- {
- btSoftBody::Node* n0 = f.m_n[0];
- btSoftBody::Node* n1 = f.m_n[1];
- btSoftBody::Node* n2 = f.m_n[2];
- const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin;
- btSoftBody::DeformableFaceRigidContact c;
- btVector3 contact_point;
- 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.
- const btScalar ms = ima + imb;
- if (ms > 0)
- {
- // resolve contact at x_n
-// psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
- btSoftBody::sCti& cti = c.m_cti;
- c.m_contactPoint = contact_point;
- c.m_bary = bary;
- // 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) + bary.length2()) * bary;
- c.m_face = &f;
+
+ //
+ // CollideSDF_RDF
+ //
+ struct CollideSDF_RDF : btDbvt::ICollide
+ {
+ void Process(const btDbvtNode* leaf)
+ {
+ btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
+ DoNode(*face);
+ }
+ void DoNode(btSoftBody::Face& f) const
+ {
+ btSoftBody::Node* n0 = f.m_n[0];
+ btSoftBody::Node* n1 = f.m_n[1];
+ btSoftBody::Node* n2 = f.m_n[2];
+ const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0) ? dynmargin : stamargin;
+ btSoftBody::DeformableFaceRigidContact c;
+ btVector3 contact_point;
+ btVector3 bary;
+ if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
+ {
+ 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.
+ const btScalar ms = ima + imb;
+ if (ms > 0)
+ {
+ // resolve contact at x_n
+ // psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
+ btSoftBody::sCti& cti = c.m_cti;
+ c.m_contactPoint = contact_point;
+ c.m_bary = bary;
+ // 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) + bary.length2()) * bary;
+ c.m_face = &f;
// friction is handled by the nodes to prevent sticking
-// const btScalar fc = 0;
- const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
-
- // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
- ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im;
- c.m_c2 = ima;
- c.m_c3 = fc;
- c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
- if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
- {
- const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
- static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
- const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
- const btVector3 ra = contact_point - wtr.getOrigin();
-
- // we do not scale the impulse matrix by dt
- c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
- c.m_c1 = ra;
- }
- else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
- {
- btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
- if (multibodyLinkCol)
- {
- btVector3 normal = cti.m_normal;
- btVector3 t1 = generateUnitOrthogonalVector(normal);
- btVector3 t2 = btCross(normal, t1);
- btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
- findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
- findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
- findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
-
- btScalar* J_n = &jacobianData_normal.m_jacobians[0];
- btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
- btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
-
- btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
- btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
- btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
-
- btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
- t1.getX(), t1.getY(), t1.getZ(),
- t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
- const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
- btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
- c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
- c.jacobianData_normal = jacobianData_normal;
- c.jacobianData_t1 = jacobianData_t1;
- c.jacobianData_t2 = jacobianData_t2;
- c.t1 = t1;
- c.t2 = t2;
- }
- }
- psb->m_faceRigidContacts.push_back(c);
- }
- }
- else
- {
- f.m_pcontact[3] = 0;
- }
- }
- btSoftBody* psb;
- const btCollisionObjectWrapper* m_colObj1Wrap;
- btRigidBody* m_rigidBody;
- btScalar dynmargin;
- btScalar stamargin;
- };
-
+ // const btScalar fc = 0;
+ const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+
+ // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
+ ima = bary.getX() * c.m_weights.getX() * n0->m_im + bary.getY() * c.m_weights.getY() * n1->m_im + bary.getZ() * c.m_weights.getZ() * n2->m_im;
+ c.m_c2 = ima;
+ c.m_c3 = fc;
+ c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+ c.m_c5 = Diagonal(ima);
+ if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+ {
+ const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+ static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+ const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+ const btVector3 ra = contact_point - wtr.getOrigin();
+
+ // we do not scale the impulse matrix by dt
+ c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+ c.m_c1 = ra;
+ }
+ else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+ {
+ btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+ if (multibodyLinkCol)
+ {
+ btVector3 normal = cti.m_normal;
+ btVector3 t1 = generateUnitOrthogonalVector(normal);
+ btVector3 t2 = btCross(normal, t1);
+ btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+ findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
+ findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
+ findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
+
+ btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+ btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+ btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+
+ btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+ btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+ btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+
+ btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+ t1.getX(), t1.getY(), t1.getZ(),
+ t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
+ const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+ btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+ c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
+ c.jacobianData_normal = jacobianData_normal;
+ c.jacobianData_t1 = jacobianData_t1;
+ c.jacobianData_t2 = jacobianData_t2;
+ c.t1 = t1;
+ c.t2 = t2;
+ }
+ }
+ psb->m_faceRigidContacts.push_back(c);
+ }
+ }
+ // Set caching barycenters to be false after collision detection.
+ // Only turn on when contact is static.
+ f.m_pcontact[3] = 0;
+ }
+ btSoftBody* psb;
+ const btCollisionObjectWrapper* m_colObj1Wrap;
+ btRigidBody* m_rigidBody;
+ btScalar dynmargin;
+ btScalar stamargin;
+ };
+
//
// CollideVF_SS
//
@@ -1844,12 +1859,12 @@ struct btSoftColliders
{
btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
- for (int i = 0; i < 3; ++i)
- {
- if (face->m_n[i] == node)
- continue;
- }
-
+ for (int i = 0; i < 3; ++i)
+ {
+ if (face->m_n[i] == node)
+ continue;
+ }
+
btVector3 o = node->m_x;
btVector3 p;
btScalar d = SIMD_INFINITY;
@@ -1879,7 +1894,7 @@ struct btSoftColliders
c.m_node = node;
c.m_face = face;
c.m_weights = w;
- c.m_friction = btMax (psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
+ c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR;
c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR;
psb[0]->m_scontacts.push_back(c);
@@ -1889,206 +1904,205 @@ 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 bary;
- if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary))
- {
- const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
- const btVector3 w = bary;
- 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 = face->m_normal;
- if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
- c.m_normal = -face->m_normal;
- c.m_margin = mrg;
- c.m_node = node;
- c.m_face = face;
- c.m_bary = w;
- c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
- psb[0]->m_faceNodeContacts.push_back(c);
- }
- }
- }
- btSoftBody* psb[2];
- btScalar mrg;
- bool useFaceNormal;
- };
-
- //
- // CollideFF_DD
- //
- struct CollideFF_DD : btDbvt::ICollide
- {
- void Process(const btDbvntNode* lface1,
- const btDbvntNode* lface2)
- {
- btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
- btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
- if (f1 != f2)
- {
- Repel(f1, f2);
- Repel(f2, f1);
- }
- }
- void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
- {
- //#define REPEL_NEIGHBOR 1
+
+ //
+ // 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 bary;
+ if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary))
+ {
+ const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
+ const btVector3 w = bary;
+ 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 = face->m_normal;
+ if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+ c.m_normal = -face->m_normal;
+ c.m_margin = mrg;
+ c.m_node = node;
+ c.m_face = face;
+ c.m_bary = w;
+ c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
+ psb[0]->m_faceNodeContacts.push_back(c);
+ }
+ }
+ }
+ btSoftBody* psb[2];
+ btScalar mrg;
+ bool useFaceNormal;
+ };
+
+ //
+ // CollideFF_DD
+ //
+ struct CollideFF_DD : btDbvt::ICollide
+ {
+ void Process(const btDbvntNode* lface1,
+ const btDbvntNode* lface2)
+ {
+ btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
+ btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
+ if (f1 != f2)
+ {
+ Repel(f1, f2);
+ Repel(f2, f1);
+ }
+ }
+ void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
+ {
+ //#define REPEL_NEIGHBOR 1
#ifndef REPEL_NEIGHBOR
- for (int node_id = 0; node_id < 3; ++node_id)
- {
- btSoftBody::Node* node = f1->m_n[node_id];
- for (int i = 0; i < 3; ++i)
- {
- if (f2->m_n[i] == node)
- return;
- }
- }
+ for (int node_id = 0; node_id < 3; ++node_id)
+ {
+ btSoftBody::Node* node = f1->m_n[node_id];
+ for (int i = 0; i < 3; ++i)
+ {
+ if (f2->m_n[i] == node)
+ return;
+ }
+ }
#endif
- bool skip = false;
- for (int node_id = 0; node_id < 3; ++node_id)
- {
- btSoftBody::Node* node = f1->m_n[node_id];
+ bool skip = false;
+ for (int node_id = 0; node_id < 3; ++node_id)
+ {
+ btSoftBody::Node* node = f1->m_n[node_id];
#ifdef REPEL_NEIGHBOR
- for (int i = 0; i < 3; ++i)
- {
- if (f2->m_n[i] == node)
- {
- skip = true;
- break;
- }
- }
- if (skip)
- {
- skip = false;
- continue;
- }
+ for (int i = 0; i < 3; ++i)
+ {
+ if (f2->m_n[i] == node)
+ {
+ skip = true;
+ break;
+ }
+ }
+ if (skip)
+ {
+ skip = false;
+ continue;
+ }
#endif
- btSoftBody::Face* face = f2;
- btVector3 bary;
- if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary))
- continue;
- btSoftBody::DeformableFaceNodeContact c;
- c.m_normal = face->m_normal;
- if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
- c.m_normal = -face->m_normal;
- c.m_margin = mrg;
- c.m_node = node;
- c.m_face = face;
- c.m_bary = bary;
- c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
- psb[0]->m_faceNodeContacts.push_back(c);
- }
- }
- btSoftBody* psb[2];
- btScalar mrg;
- bool useFaceNormal;
- };
-
- struct CollideCCD : 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 bary;
- if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
- {
- btSoftBody::DeformableFaceNodeContact c;
- c.m_normal = face->m_normal;
- if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
- c.m_normal = -face->m_normal;
- c.m_node = node;
- c.m_face = face;
- c.m_bary = bary;
- c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
- psb[0]->m_faceNodeContacts.push_back(c);
- }
- }
- void Process(const btDbvntNode* lface1,
- const btDbvntNode* lface2)
- {
- btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
- btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
- if (f1 != f2)
- {
- Repel(f1, f2);
- Repel(f2, f1);
- }
- }
- void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
- {
- //#define REPEL_NEIGHBOR 1
+ btSoftBody::Face* face = f2;
+ btVector3 bary;
+ if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary))
+ continue;
+ btSoftBody::DeformableFaceNodeContact c;
+ c.m_normal = face->m_normal;
+ if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+ c.m_normal = -face->m_normal;
+ c.m_margin = mrg;
+ c.m_node = node;
+ c.m_face = face;
+ c.m_bary = bary;
+ c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
+ psb[0]->m_faceNodeContacts.push_back(c);
+ }
+ }
+ btSoftBody* psb[2];
+ btScalar mrg;
+ bool useFaceNormal;
+ };
+
+ struct CollideCCD : 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 bary;
+ if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
+ {
+ btSoftBody::DeformableFaceNodeContact c;
+ c.m_normal = face->m_normal;
+ if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+ c.m_normal = -face->m_normal;
+ c.m_node = node;
+ c.m_face = face;
+ c.m_bary = bary;
+ c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
+ psb[0]->m_faceNodeContacts.push_back(c);
+ }
+ }
+ void Process(const btDbvntNode* lface1,
+ const btDbvntNode* lface2)
+ {
+ btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
+ btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
+ if (f1 != f2)
+ {
+ Repel(f1, f2);
+ Repel(f2, f1);
+ }
+ }
+ void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
+ {
+ //#define REPEL_NEIGHBOR 1
#ifndef REPEL_NEIGHBOR
- for (int node_id = 0; node_id < 3; ++node_id)
- {
- btSoftBody::Node* node = f1->m_n[node_id];
- for (int i = 0; i < 3; ++i)
- {
- if (f2->m_n[i] == node)
- return;
- }
- }
+ for (int node_id = 0; node_id < 3; ++node_id)
+ {
+ btSoftBody::Node* node = f1->m_n[node_id];
+ for (int i = 0; i < 3; ++i)
+ {
+ if (f2->m_n[i] == node)
+ return;
+ }
+ }
#endif
- bool skip = false;
- for (int node_id = 0; node_id < 3; ++node_id)
- {
- btSoftBody::Node* node = f1->m_n[node_id];
+ bool skip = false;
+ for (int node_id = 0; node_id < 3; ++node_id)
+ {
+ btSoftBody::Node* node = f1->m_n[node_id];
#ifdef REPEL_NEIGHBOR
- for (int i = 0; i < 3; ++i)
- {
- if (f2->m_n[i] == node)
- {
- skip = true;
- break;
- }
- }
- if (skip)
- {
- skip = false;
- continue;
- }
+ for (int i = 0; i < 3; ++i)
+ {
+ if (f2->m_n[i] == node)
+ {
+ skip = true;
+ break;
+ }
+ }
+ if (skip)
+ {
+ skip = false;
+ continue;
+ }
#endif
- btSoftBody::Face* face = f2;
- btVector3 bary;
+ btSoftBody::Face* face = f2;
+ btVector3 bary;
if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
- {
- btSoftBody::DeformableFaceNodeContact c;
- c.m_normal = face->m_normal;
- if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
- c.m_normal = -face->m_normal;
- c.m_node = node;
- c.m_face = face;
- c.m_bary = bary;
- c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
- psb[0]->m_faceNodeContacts.push_back(c);
- }
- }
- }
- btSoftBody* psb[2];
- btScalar dt, mrg;
- bool useFaceNormal;
- };
+ {
+ btSoftBody::DeformableFaceNodeContact c;
+ c.m_normal = face->m_normal;
+ if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
+ c.m_normal = -face->m_normal;
+ c.m_node = node;
+ c.m_face = face;
+ c.m_bary = bary;
+ c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
+ psb[0]->m_faceNodeContacts.push_back(c);
+ }
+ }
+ }
+ btSoftBody* psb[2];
+ btScalar dt, mrg;
+ bool useFaceNormal;
+ };
};
#endif //_BT_SOFT_BODY_INTERNALS_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-07 18:44:17 +0000