bullet: Sync with upstream 3.07

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