virtualx-engine/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///btSoftBody implementation by Nathanael Presson

#ifndef _BT_SOFT_BODY_INTERNALS_H
#define _BT_SOFT_BODY_INTERNALS_H

#include "btSoftBody.h"

#include "LinearMath/btQuickprof.h"
#include "LinearMath/btPolarDecomposition.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include <string.h>  //for memset
#include <cmath>

// 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 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);
}
static 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;
}
//
// btSymMatrix
//
template <typename T>
struct btSymMatrix
{
	btSymMatrix() : dim(0) {}
	btSymMatrix(int n, const T& init = T()) { resize(n, init); }
	void resize(int n, const T& init = T())
	{
		dim = n;
		store.resize((n * (n + 1)) / 2, init);
	}
	int index(int c, int r) const
	{
		if (c > r) btSwap(c, r);
		btAssert(r < dim);
		return ((r * (r + 1)) / 2 + c);
	}
	T& operator()(int c, int r) { return (store[index(c, r)]); }
	const T& operator()(int c, int r) const { return (store[index(c, r)]); }
	btAlignedObjectArray<T> store;
	int dim;
};

//
// btSoftBodyCollisionShape
//
class btSoftBodyCollisionShape : public btConcaveShape
{
public:
	btSoftBody* m_body;

	btSoftBodyCollisionShape(btSoftBody* backptr)
	{
		m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
		m_body = backptr;
	}

	virtual ~btSoftBodyCollisionShape()
	{
	}

	void processAllTriangles(btTriangleCallback* /*callback*/, const btVector3& /*aabbMin*/, const btVector3& /*aabbMax*/) const
	{
		//not yet
		btAssert(0);
	}

	///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
	{
		/* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */
		const btVector3 mins = m_body->m_bounds[0];
		const btVector3 maxs = m_body->m_bounds[1];
		const btVector3 crns[] = {t * btVector3(mins.x(), mins.y(), mins.z()),
								  t * btVector3(maxs.x(), mins.y(), mins.z()),
								  t * btVector3(maxs.x(), maxs.y(), mins.z()),
								  t * btVector3(mins.x(), maxs.y(), mins.z()),
								  t * btVector3(mins.x(), mins.y(), maxs.z()),
								  t * btVector3(maxs.x(), mins.y(), maxs.z()),
								  t * btVector3(maxs.x(), maxs.y(), maxs.z()),
								  t * btVector3(mins.x(), maxs.y(), maxs.z())};
		aabbMin = aabbMax = crns[0];
		for (int i = 1; i < 8; ++i)
		{
			aabbMin.setMin(crns[i]);
			aabbMax.setMax(crns[i]);
		}
	}

	virtual void setLocalScaling(const btVector3& /*scaling*/)
	{
		///na
	}
	virtual const btVector3& getLocalScaling() const
	{
		static const btVector3 dummy(1, 1, 1);
		return dummy;
	}
	virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const
	{
		///not yet
		btAssert(0);
	}
	virtual const char* getName() const
	{
		return "SoftBody";
	}
};

//
// btSoftClusterCollisionShape
//
class btSoftClusterCollisionShape : public btConvexInternalShape
{
public:
	const btSoftBody::Cluster* m_cluster;

	btSoftClusterCollisionShape(const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }

	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
	{
		btSoftBody::Node* const* n = &m_cluster->m_nodes[0];
		btScalar d = btDot(vec, n[0]->m_x);
		int j = 0;
		for (int i = 1, ni = m_cluster->m_nodes.size(); i < ni; ++i)
		{
			const btScalar k = btDot(vec, n[i]->m_x);
			if (k > d)
			{
				d = k;
				j = i;
			}
		}
		return (n[j]->m_x);
	}
	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
	{
		return (localGetSupportingVertex(vec));
	}
	//notice that the vectors should be unit length
	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
	{
	}

	virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const
	{
	}

	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
	{
	}

	virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }

	//debugging
	virtual const char* getName() const { return "SOFTCLUSTER"; }

	virtual void setMargin(btScalar margin)
	{
		btConvexInternalShape::setMargin(margin);
	}
	virtual btScalar getMargin() const
	{
		return btConvexInternalShape::getMargin();
	}
};

//
// Inline's
//

//
template <typename T>
static inline void ZeroInitialize(T& value)
{
	memset(&value, 0, sizeof(T));
}
//
template <typename T>
static inline bool CompLess(const T& a, const T& b)
{
	return (a < b);
}
//
template <typename T>
static inline bool CompGreater(const T& a, const T& b)
{
	return (a > b);
}
//
template <typename T>
static inline T Lerp(const T& a, const T& b, btScalar t)
{
	return (a + (b - a) * t);
}
//
template <typename T>
static inline T InvLerp(const T& a, const T& b, btScalar t)
{
	return ((b + a * t - b * t) / (a * b));
}
//
static inline btMatrix3x3 Lerp(const btMatrix3x3& a,
							   const btMatrix3x3& b,
							   btScalar t)
{
	btMatrix3x3 r;
	r[0] = Lerp(a[0], b[0], t);
	r[1] = Lerp(a[1], b[1], t);
	r[2] = Lerp(a[2], b[2], t);
	return (r);
}
//
static inline btVector3 Clamp(const btVector3& v, btScalar maxlength)
{
	const btScalar sql = v.length2();
	if (sql > (maxlength * maxlength))
		return ((v * maxlength) / btSqrt(sql));
	else
		return (v);
}
//
template <typename T>
static inline T Clamp(const T& x, const T& l, const T& h)
{
	return (x < l ? l : x > h ? h : x);
}
//
template <typename T>
static inline T Sq(const T& x)
{
	return (x * x);
}
//
template <typename T>
static inline T Cube(const T& x)
{
	return (x * x * x);
}
//
template <typename T>
static inline T Sign(const T& x)
{
	return ((T)(x < 0 ? -1 : +1));
}
//
template <typename T>
static inline bool SameSign(const T& x, const T& y)
{
	return ((x * y) > 0);
}
//
static inline btScalar ClusterMetric(const btVector3& x, const btVector3& y)
{
	const btVector3 d = x - y;
	return (btFabs(d[0]) + btFabs(d[1]) + btFabs(d[2]));
}
//
static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a, btScalar s)
{
	const btScalar xx = a.x() * a.x();
	const btScalar yy = a.y() * a.y();
	const btScalar zz = a.z() * a.z();
	const btScalar xy = a.x() * a.y();
	const btScalar yz = a.y() * a.z();
	const btScalar zx = a.z() * a.x();
	btMatrix3x3 m;
	m[0] = btVector3(1 - xx + xx * s, xy * s - xy, zx * s - zx);
	m[1] = btVector3(xy * s - xy, 1 - yy + yy * s, yz * s - yz);
	m[2] = btVector3(zx * s - zx, yz * s - yz, 1 - zz + zz * s);
	return (m);
}
//
static inline btMatrix3x3 Cross(const btVector3& v)
{
	btMatrix3x3 m;
	m[0] = btVector3(0, -v.z(), +v.y());
	m[1] = btVector3(+v.z(), 0, -v.x());
	m[2] = btVector3(-v.y(), +v.x(), 0);
	return (m);
}
//
static inline btMatrix3x3 Diagonal(btScalar x)
{
	btMatrix3x3 m;
	m[0] = btVector3(x, 0, 0);
	m[1] = btVector3(0, x, 0);
	m[2] = btVector3(0, 0, x);
	return (m);
}

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 Add(const btMatrix3x3& a,
							  const btMatrix3x3& b)
{
	btMatrix3x3 r;
	for (int i = 0; i < 3; ++i) r[i] = a[i] + b[i];
	return (r);
}
//
static inline btMatrix3x3 Sub(const btMatrix3x3& a,
							  const btMatrix3x3& b)
{
	btMatrix3x3 r;
	for (int i = 0; i < 3; ++i) r[i] = a[i] - b[i];
	return (r);
}
//
static inline btMatrix3x3 Mul(const btMatrix3x3& a,
							  btScalar b)
{
	btMatrix3x3 r;
	for (int i = 0; i < 3; ++i) r[i] = a[i] * b;
	return (r);
}
//
static inline void Orthogonalize(btMatrix3x3& m)
{
	m[2] = btCross(m[0], m[1]).normalized();
	m[1] = btCross(m[2], m[0]).normalized();
	m[0] = btCross(m[1], m[2]).normalized();
}
//
static inline btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3& iwi, const btVector3& r)
{
	const btMatrix3x3 cr = Cross(r);
	return (Sub(Diagonal(im), cr * iwi * cr));
}

//
static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
										btScalar ima,
										btScalar imb,
										const btMatrix3x3& iwi,
										const btVector3& r)
{
	return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse());
}

//
static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
										btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
{
	return (Add(MassMatrix(ima, iia, ra), MassMatrix(imb, iib, rb)).inverse());
}

//
static inline btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3& iia,
											   const btMatrix3x3& iib)
{
	return (Add(iia, iib).inverse());
}

//
static inline btVector3 ProjectOnAxis(const btVector3& v,
									  const btVector3& a)
{
	return (a * btDot(v, a));
}
//
static inline btVector3 ProjectOnPlane(const btVector3& v,
									   const btVector3& a)
{
	return (v - ProjectOnAxis(v, a));
}

//
static inline void ProjectOrigin(const btVector3& a,
								 const btVector3& b,
								 btVector3& prj,
								 btScalar& sqd)
{
	const btVector3 d = b - a;
	const btScalar m2 = d.length2();
	if (m2 > SIMD_EPSILON)
	{
		const btScalar t = Clamp<btScalar>(-btDot(a, d) / m2, 0, 1);
		const btVector3 p = a + d * t;
		const btScalar l2 = p.length2();
		if (l2 < sqd)
		{
			prj = p;
			sqd = l2;
		}
	}
}
//
static inline void ProjectOrigin(const btVector3& a,
								 const btVector3& b,
								 const btVector3& c,
								 btVector3& prj,
								 btScalar& sqd)
{
	const btVector3& q = btCross(b - a, c - a);
	const btScalar m2 = q.length2();
	if (m2 > SIMD_EPSILON)
	{
		const btVector3 n = q / btSqrt(m2);
		const btScalar k = btDot(a, n);
		const btScalar k2 = k * k;
		if (k2 < sqd)
		{
			const btVector3 p = n * k;
			if ((btDot(btCross(a - p, b - p), q) > 0) &&
				(btDot(btCross(b - p, c - p), q) > 0) &&
				(btDot(btCross(c - p, a - p), q) > 0))
			{
				prj = p;
				sqd = k2;
			}
			else
			{
				ProjectOrigin(a, b, prj, sqd);
				ProjectOrigin(b, c, prj, sqd);
				ProjectOrigin(c, a, prj, sqd);
			}
		}
	}
}

//
static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t)
{
    btScalar a, f, u, v;
    
    btVector3 e1 = v1 - v0;
    btVector3 e2 = v2 - v0;
    btVector3 h = dir.cross(e2);
    a = e1.dot(h);
    
    if (a > -0.00001 && a < 0.00001)
        return (false);
    
    f = btScalar(1) / a;
    btVector3 s = origin - v0;
    u = f * s.dot(h);
    
    if (u < 0.0 || u > 1.0)
        return (false);
    
    btVector3 q = s.cross(e1);
    v = f * dir.dot(q);
    if (v < 0.0 || u + v > 1.0)
        return (false);
    // at this stage we can compute t to find out where
    // the intersection point is on the line
    t = f * e2.dot(q);
    if (t > 0)  // ray intersection
        return (true);
    else  // this means that there is a line intersection
        // but not a ray intersection
        return (false);
}

static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal)
{
    btVector3 dir = rayEnd - rayStart;
    btScalar dir_norm = dir.norm();
    if (dir_norm < SIMD_EPSILON)
        return false;
    dir.normalize();

    btScalar t;
    
    bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
    
    if (ret)
    {
        if (t <= dir_norm)
        {
            sect = rayStart + dir * t;
        }
        else
        {
            ret = false;
        }
    }
    
    if (ret)
    {
        btVector3 n = (p3-p1).cross(p2-p1);
        n.safeNormalize();
        if (n.dot(dir) < 0)
            normal = n;
        else
            normal = -n;
    }
    return ret;
}


//
template <typename T>
static inline T BaryEval(const T& a,
						 const T& b,
						 const T& c,
						 const btVector3& coord)
{
	return (a * coord.x() + b * coord.y() + c * coord.z());
}
//
static inline btVector3 BaryCoord(const btVector3& a,
								  const btVector3& b,
								  const btVector3& c,
								  const btVector3& p)
{
	const btScalar w[] = {btCross(a - p, b - p).length(),
						  btCross(b - p, c - p).length(),
						  btCross(c - p, a - p).length()};
	const btScalar isum = 1 / (w[0] + w[1] + w[2]);
	return (btVector3(w[1] * isum, w[2] * isum, w[0] * isum));
}

//
inline static btScalar ImplicitSolve(btSoftBody::ImplicitFn* fn,
									 const btVector3& a,
									 const btVector3& b,
									 const btScalar accuracy,
									 const int maxiterations = 256)
{
	btScalar span[2] = {0, 1};
	btScalar values[2] = {fn->Eval(a), fn->Eval(b)};
	if (values[0] > values[1])
	{
		btSwap(span[0], span[1]);
		btSwap(values[0], values[1]);
	}
	if (values[0] > -accuracy) return (-1);
	if (values[1] < +accuracy) return (-1);
	for (int i = 0; i < maxiterations; ++i)
	{
		const btScalar t = Lerp(span[0], span[1], values[0] / (values[0] - values[1]));
		const btScalar v = fn->Eval(Lerp(a, b, t));
		if ((t <= 0) || (t >= 1)) break;
		if (btFabs(v) < accuracy) return (t);
		if (v < 0)
		{
			span[0] = t;
			values[0] = v;
		}
		else
		{
			span[1] = t;
			values[1] = v;
		}
	}
	return (-1);
}

inline static void EvaluateMedium(const btSoftBodyWorldInfo* wfi,
								  const btVector3& x,
								  btSoftBody::sMedium& medium)
{
	medium.m_velocity = btVector3(0, 0, 0);
	medium.m_pressure = 0;
	medium.m_density = wfi->air_density;
	if (wfi->water_density > 0)
	{
		const btScalar depth = -(btDot(x, wfi->water_normal) + wfi->water_offset);
		if (depth > 0)
		{
			medium.m_density = wfi->water_density;
			medium.m_pressure = depth * wfi->water_density * wfi->m_gravity.length();
		}
	}
}

//
static inline btVector3 NormalizeAny(const btVector3& v)
{
	const btScalar l = v.length();
	if (l > SIMD_EPSILON)
		return (v / l);
	else
		return (btVector3(0, 0, 0));
}

//
static inline btDbvtVolume VolumeOf(const btSoftBody::Face& f,
									btScalar margin)
{
	const btVector3* pts[] = {&f.m_n[0]->m_x,
							  &f.m_n[1]->m_x,
							  &f.m_n[2]->m_x};
	btDbvtVolume vol = btDbvtVolume::FromPoints(pts, 3);
	vol.Expand(btVector3(margin, margin, margin));
	return (vol);
}

//
static inline btVector3 CenterOf(const btSoftBody::Face& f)
{
	return ((f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3);
}

//
static inline btScalar AreaOf(const btVector3& x0,
							  const btVector3& x1,
							  const btVector3& x2)
{
	const btVector3 a = x1 - x0;
	const btVector3 b = x2 - x0;
	const btVector3 cr = btCross(a, b);
	const btScalar area = cr.length();
	return (area);
}

//
static inline btScalar VolumeOf(const btVector3& x0,
								const btVector3& x1,
								const btVector3& x2,
								const btVector3& x3)
{
	const btVector3 a = x1 - x0;
	const btVector3 b = x2 - x0;
	const btVector3 c = x3 - x0;
	return (btDot(a, btCross(b, c)));
}

//

//
static inline void ApplyClampedForce(btSoftBody::Node& n,
									 const btVector3& f,
									 btScalar dt)
{
	const btScalar dtim = dt * n.m_im;
	if ((f * dtim).length2() > n.m_v.length2())
	{ /* Clamp	*/
		n.m_f -= ProjectOnAxis(n.m_v, f.normalized()) / dtim;
	}
	else
	{ /* Apply	*/
		n.m_f += f;
	}
}

//
static inline int MatchEdge(const btSoftBody::Node* a,
							const btSoftBody::Node* b,
							const btSoftBody::Node* ma,
							const btSoftBody::Node* mb)
{
	if ((a == ma) && (b == mb)) return (0);
	if ((a == mb) && (b == ma)) return (1);
	return (-1);
}

//
// btEigen : Extract eigen system,
// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
// outputs are NOT sorted.
//
struct btEigen
{
	static int system(btMatrix3x3& a, btMatrix3x3* vectors, btVector3* values = 0)
	{
		static const int maxiterations = 16;
		static const btScalar accuracy = (btScalar)0.0001;
		btMatrix3x3& v = *vectors;
		int iterations = 0;
		vectors->setIdentity();
		do
		{
			int p = 0, q = 1;
			if (btFabs(a[p][q]) < btFabs(a[0][2]))
			{
				p = 0;
				q = 2;
			}
			if (btFabs(a[p][q]) < btFabs(a[1][2]))
			{
				p = 1;
				q = 2;
			}
			if (btFabs(a[p][q]) > accuracy)
			{
				const btScalar w = (a[q][q] - a[p][p]) / (2 * a[p][q]);
				const btScalar z = btFabs(w);
				const btScalar t = w / (z * (btSqrt(1 + w * w) + z));
				if (t == t) /* [WARNING] let hope that one does not get thrown aways by some compilers... */
				{
					const btScalar c = 1 / btSqrt(t * t + 1);
					const btScalar s = c * t;
					mulPQ(a, c, s, p, q);
					mulTPQ(a, c, s, p, q);
					mulPQ(v, c, s, p, q);
				}
				else
					break;
			}
			else
				break;
		} while ((++iterations) < maxiterations);
		if (values)
		{
			*values = btVector3(a[0][0], a[1][1], a[2][2]);
		}
		return (iterations);
	}

private:
	static inline void mulTPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
	{
		const btScalar m[2][3] = {{a[p][0], a[p][1], a[p][2]},
								  {a[q][0], a[q][1], a[q][2]}};
		int i;

		for (i = 0; i < 3; ++i) a[p][i] = c * m[0][i] - s * m[1][i];
		for (i = 0; i < 3; ++i) a[q][i] = c * m[1][i] + s * m[0][i];
	}
	static inline void mulPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
	{
		const btScalar m[2][3] = {{a[0][p], a[1][p], a[2][p]},
								  {a[0][q], a[1][q], a[2][q]}};
		int i;

		for (i = 0; i < 3; ++i) a[i][p] = c * m[0][i] - s * m[1][i];
		for (i = 0; i < 3; ++i) a[i][q] = c * m[1][i] + s * m[0][i];
	}
};

//
// Polar decomposition,
// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
//
static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
{
	static const btPolarDecomposition polar;
	return polar.decompose(m, q, s);
}

//
// btSoftColliders
//
struct btSoftColliders
{
	//
	// ClusterBase
	//
	struct ClusterBase : btDbvt::ICollide
	{
		btScalar erp;
		btScalar idt;
		btScalar m_margin;
		btScalar friction;
		btScalar threshold;
		ClusterBase()
		{
			erp = (btScalar)1;
			idt = 0;
			m_margin = 0;
			friction = 0;
			threshold = (btScalar)0;
		}
		bool SolveContact(const btGjkEpaSolver2::sResults& res,
						  btSoftBody::Body ba, const btSoftBody::Body bb,
						  btSoftBody::CJoint& joint)
		{
			if (res.distance < m_margin)
			{
				btVector3 norm = res.normal;
				norm.normalize();  //is it necessary?

				const btVector3 ra = res.witnesses[0] - ba.xform().getOrigin();
				const btVector3 rb = res.witnesses[1] - bb.xform().getOrigin();
				const btVector3 va = ba.velocity(ra);
				const btVector3 vb = bb.velocity(rb);
				const btVector3 vrel = va - vb;
				const btScalar rvac = btDot(vrel, norm);
				btScalar depth = res.distance - m_margin;

				//				printf("depth=%f\n",depth);
				const btVector3 iv = norm * rvac;
				const btVector3 fv = vrel - iv;
				joint.m_bodies[0] = ba;
				joint.m_bodies[1] = bb;
				joint.m_refs[0] = ra * ba.xform().getBasis();
				joint.m_refs[1] = rb * bb.xform().getBasis();
				joint.m_rpos[0] = ra;
				joint.m_rpos[1] = rb;
				joint.m_cfm = 1;
				joint.m_erp = 1;
				joint.m_life = 0;
				joint.m_maxlife = 0;
				joint.m_split = 1;

				joint.m_drift = depth * norm;

				joint.m_normal = norm;
				//				printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
				joint.m_delete = false;
				joint.m_friction = fv.length2() < (rvac * friction * rvac * friction) ? 1 : friction;
				joint.m_massmatrix = ImpulseMatrix(ba.invMass(), ba.invWorldInertia(), joint.m_rpos[0],
												   bb.invMass(), bb.invWorldInertia(), joint.m_rpos[1]);

				return (true);
			}
			return (false);
		}
	};
	//
	// CollideCL_RS
	//
	struct CollideCL_RS : ClusterBase
	{
		btSoftBody* psb;
		const btCollisionObjectWrapper* m_colObjWrap;

		void Process(const btDbvtNode* leaf)
		{
			btSoftBody::Cluster* cluster = (btSoftBody::Cluster*)leaf->data;
			btSoftClusterCollisionShape cshape(cluster);

			const btConvexShape* rshape = (const btConvexShape*)m_colObjWrap->getCollisionShape();

			///don't collide an anchored cluster with a static/kinematic object
			if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor)
				return;

			btGjkEpaSolver2::sResults res;
			if (btGjkEpaSolver2::SignedDistance(&cshape, btTransform::getIdentity(),
												rshape, m_colObjWrap->getWorldTransform(),
												btVector3(1, 0, 0), res))
			{
				btSoftBody::CJoint joint;
				if (SolveContact(res, cluster, m_colObjWrap->getCollisionObject(), joint))  //prb,joint))
				{
					btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint();
					*pj = joint;
					psb->m_joints.push_back(pj);
					if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject())
					{
						pj->m_erp *= psb->m_cfg.kSKHR_CL;
						pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
					}
					else
					{
						pj->m_erp *= psb->m_cfg.kSRHR_CL;
						pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
					}
				}
			}
		}
		void ProcessColObj(btSoftBody* ps, const btCollisionObjectWrapper* colObWrap)
		{
			psb = ps;
			m_colObjWrap = colObWrap;
			idt = ps->m_sst.isdt;
			m_margin = m_colObjWrap->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin();
			///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
			friction = btMin(psb->m_cfg.kDF, m_colObjWrap->getCollisionObject()->getFriction());
			btVector3 mins;
			btVector3 maxs;

			ATTRIBUTE_ALIGNED16(btDbvtVolume)
			volume;
			colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(), mins, maxs);
			volume = btDbvtVolume::FromMM(mins, maxs);
			volume.Expand(btVector3(1, 1, 1) * m_margin);
			ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root, volume, *this);
		}
	};
	//
	// CollideCL_SS
	//
	struct CollideCL_SS : ClusterBase
	{
		btSoftBody* bodies[2];
		void Process(const btDbvtNode* la, const btDbvtNode* lb)
		{
			btSoftBody::Cluster* cla = (btSoftBody::Cluster*)la->data;
			btSoftBody::Cluster* clb = (btSoftBody::Cluster*)lb->data;

			bool connected = false;
			if ((bodies[0] == bodies[1]) && (bodies[0]->m_clusterConnectivity.size()))
			{
				connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex + bodies[0]->m_clusters.size() * clb->m_clusterIndex];
			}

			if (!connected)
			{
				btSoftClusterCollisionShape csa(cla);
				btSoftClusterCollisionShape csb(clb);
				btGjkEpaSolver2::sResults res;
				if (btGjkEpaSolver2::SignedDistance(&csa, btTransform::getIdentity(),
													&csb, btTransform::getIdentity(),
													cla->m_com - clb->m_com, res))
				{
					btSoftBody::CJoint joint;
					if (SolveContact(res, cla, clb, joint))
					{
						btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint();
						*pj = joint;
						bodies[0]->m_joints.push_back(pj);
						pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL, bodies[1]->m_cfg.kSSHR_CL);
						pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL + bodies[1]->m_cfg.kSS_SPLT_CL) / 2;
					}
				}
			}
			else
			{
				static int count = 0;
				count++;
				//printf("count=%d\n",count);
			}
		}
		void ProcessSoftSoft(btSoftBody* psa, btSoftBody* psb)
		{
			idt = psa->m_sst.isdt;
			//m_margin		=	(psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
			m_margin = (psa->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin());
			friction = btMin(psa->m_cfg.kDF, psb->m_cfg.kDF);
			bodies[0] = psa;
			bodies[1] = psb;
			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_RD
	//
	struct CollideSDF_RD : 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::DeformableNodeRigidContact c;

			if (!n.m_battach)
            {
                // check for collision at x_{n+1}^* as well at x_n
                if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || 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);
                    }
                }
			}
		}
		btSoftBody* psb;
		const btCollisionObjectWrapper* m_colObj1Wrap;
		btRigidBody* m_rigidBody;
		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;
                    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;
    };
    
	//
	// CollideVF_SS
	//
	struct CollideVF_SS : btDbvt::ICollide
	{
		void Process(const btDbvtNode* lnode,
					 const btDbvtNode* lface)
		{
			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;
            }
            
			btVector3 o = node->m_x;
			btVector3 p;
			btScalar d = SIMD_INFINITY;
			ProjectOrigin(face->m_n[0]->m_x - o,
						  face->m_n[1]->m_x - o,
						  face->m_n[2]->m_x - o,
						  p, d);
			const btScalar m = mrg + (o - node->m_q).length() * 2;
			if (d < (m * m))
			{
				const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
				const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
				const btScalar ma = node->m_im;
				btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
				if ((n[0]->m_im <= 0) ||
					(n[1]->m_im <= 0) ||
					(n[2]->m_im <= 0))
				{
					mb = 0;
				}
				const btScalar ms = ma + mb;
				if (ms > 0)
				{
					btSoftBody::SContact c;
					c.m_normal = p / -btSqrt(d);
					c.m_margin = m;
					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_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);
				}
			}
		}
		btSoftBody* psb[2];
		btScalar mrg;
	};
    
    
    //
    // CollideVF_DD
    //
    struct CollideVF_DD : btDbvt::ICollide
    {
        void Process(const btDbvtNode* lnode,
                     const btDbvtNode* lface)
        {
            btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
            btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
            
            btVector3 o = node->m_x;
            btVector3 p;
            btScalar d = SIMD_INFINITY;
            ProjectOrigin(face->m_n[0]->m_x - o,
                          face->m_n[1]->m_x - o,
                          face->m_n[2]->m_x - o,
                          p, d);
            const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
            if (d < (m * m))
            {
                const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
                const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
                const btScalar ma = node->m_im;
                btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
                if ((n[0]->m_im <= 0) ||
                    (n[1]->m_im <= 0) ||
                    (n[2]->m_im <= 0))
                {
                    mb = 0;
                }
                const btScalar ms = ma + mb;
                if (ms > 0)
                {
                    btSoftBody::DeformableFaceNodeContact c;
                    if (useFaceNormal)
                        c.m_normal = face->m_normal;
                    else
                        c.m_normal = p / -btSqrt(d);
                    c.m_margin = mrg;
                    c.m_node = node;
                    c.m_face = face;
                    c.m_bary = w;
                    // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
                    c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
                    c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
                    // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
                    c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
                    c.m_c0 = btScalar(1)/(ma + c.m_imf);
                    psb[0]->m_faceNodeContacts.push_back(c);
                }
            }
        }
        btSoftBody* psb[2];
        btScalar mrg;
        bool useFaceNormal;
    };
    
    //
    // CollideFF_DD
    //
    struct CollideFF_DD : btDbvt::ICollide
    {
        void Process(const btDbvntNode* lface1,
                     const btDbvntNode* lface2)
        {
            btSoftBody::Face* f = (btSoftBody::Face*)lface1->data;
            btSoftBody::Face* face = (btSoftBody::Face*)lface2->data;
            for (int node_id = 0; node_id < 3; ++node_id)
            {
                btSoftBody::Node* node = f->m_n[node_id];
                bool skip = false;
                for (int i = 0; i < 3; ++i)
                {
                    if (face->m_n[i] == node)
                    {
                        skip = true;
                        break;
                    }
                }
                if (skip)
                    continue;
                btVector3 o = node->m_x;
                btVector3 p;
                btScalar d = SIMD_INFINITY;
                ProjectOrigin(face->m_n[0]->m_x - o,
                              face->m_n[1]->m_x - o,
                              face->m_n[2]->m_x - o,
                              p, d);
                const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
                if (d < (m * m))
                {
                    const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
                    const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
                    const btScalar ma = node->m_im;
                    btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
                    if ((n[0]->m_im <= 0) ||
                        (n[1]->m_im <= 0) ||
                        (n[2]->m_im <= 0))
                    {
                        mb = 0;
                    }
                    const btScalar ms = ma + mb;
                    if (ms > 0)
                    {
                        btSoftBody::DeformableFaceNodeContact c;
                        if (useFaceNormal)
                            c.m_normal = face->m_normal;
                        else
                            c.m_normal = p / -btSqrt(d);
                        c.m_margin = mrg;
                        c.m_node = node;
                        c.m_face = face;
                        c.m_bary = w;
                        // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
                        c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
                        c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
                        // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
                        c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
                        c.m_c0 = btScalar(1)/(ma + c.m_imf);
                        psb[0]->m_faceNodeContacts.push_back(c);
                    }
                }
            }
        }
        btSoftBody* psb[2];
        btScalar mrg;
        bool useFaceNormal;
    };
};

#endif  //_BT_SOFT_BODY_INTERNALS_H