bullet: Sync with upstream 3.07

1 files changed, 726 insertions, 687 deletions

diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp b/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp
index 81b846d7f8..d1980ea6c5 100644
--- a/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp
+++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp
@@ -37,12 +37,12 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l
 	{
 		btAlignedObjectArray<bool> marked;
 		btAlignedObjectArray<btDbvtNode*> newLeafNodes;
-		btAlignedObjectArray<std::pair<int,int> > childIds;
+		btAlignedObjectArray<std::pair<int, int> > childIds;
 		btAlignedObjectArray<btAlignedObjectArray<int> > newAdj;
 		marked.resize(N);
 		for (int i = 0; i < N; ++i)
 			marked[i] = false;
-		
+
 		// pair adjacent nodes into new(parent) node
 		for (int i = 0; i < N; ++i)
 		{
@@ -61,7 +61,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l
 					leafNodes[i]->parent = node;
 					leafNodes[n]->parent = node;
 					newLeafNodes.push_back(node);
-					childIds.push_back(std::make_pair(i,n));
+					childIds.push_back(std::make_pair(i, n));
 					merged = true;
 					marked[n] = true;
 					break;
@@ -70,7 +70,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l
 			if (!merged)
 			{
 				newLeafNodes.push_back(leafNodes[i]);
-				childIds.push_back(std::make_pair(i,-1));
+				childIds.push_back(std::make_pair(i, -1));
 			}
 			marked[i] = true;
 		}
@@ -78,7 +78,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l
 		newAdj.resize(newLeafNodes.size());
 		for (int i = 0; i < newLeafNodes.size(); ++i)
 		{
-			for (int j = i+1; j < newLeafNodes.size(); ++j)
+			for (int j = i + 1; j < newLeafNodes.size(); ++j)
 			{
 				bool neighbor = false;
 				const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first];
@@ -143,7 +143,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
 	/* Nodes			*/
 	const btScalar margin = getCollisionShape()->getMargin();
 	m_nodes.resize(node_count);
-    m_X.resize(node_count);
+	m_X.resize(node_count);
 	for (int i = 0, ni = node_count; i < ni; ++i)
 	{
 		Node& n = m_nodes[i];
@@ -154,7 +154,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
 		n.m_im = n.m_im > 0 ? 1 / n.m_im : 0;
 		n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
 		n.m_material = pm;
-        m_X[i] = n.m_x;
+		m_X[i] = n.m_x;
 	}
 	updateBounds();
 	setCollisionQuadrature(3);
@@ -195,8 +195,8 @@ void btSoftBody::initDefaults()
 	m_cfg.piterations = 1;
 	m_cfg.diterations = 0;
 	m_cfg.citerations = 4;
-    m_cfg.drag = 0;
-    m_cfg.m_maxStress = 0;
+	m_cfg.drag = 0;
+	m_cfg.m_maxStress = 0;
 	m_cfg.collisions = fCollision::Default;
 	m_pose.m_bvolume = false;
 	m_pose.m_bframe = false;
@@ -222,12 +222,14 @@ void btSoftBody::initDefaults()
 	m_windVelocity = btVector3(0, 0, 0);
 	m_restLengthScale = btScalar(1.0);
 	m_dampingCoefficient = 1.0;
-	m_sleepingThreshold = .4;
+	m_sleepingThreshold = .04;
 	m_useSelfCollision = false;
 	m_collisionFlags = 0;
 	m_softSoftCollision = false;
 	m_maxSpeedSquared = 0;
 	m_repulsionStiffness = 0.5;
+	m_gravityFactor = 1;
+	m_cacheBarycenter = false;
 	m_fdbvnt = 0;
 }
 
@@ -436,7 +438,7 @@ void btSoftBody::appendFace(int model, Material* mat)
 		ZeroInitialize(f);
 		f.m_material = mat ? mat : m_materials[0];
 	}
-    m_faces.push_back(f);
+	m_faces.push_back(f);
 }
 
 //
@@ -525,94 +527,111 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca
 //
 void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
 {
-    DeformableNodeRigidAnchor c;
-    btSoftBody::Node& n = m_nodes[node];
-    const btScalar ima = n.m_im;
-    const btScalar imb = body->getInvMass();
-    btVector3 nrm;
-    const btCollisionShape* shp = body->getCollisionShape();
-    const btTransform& wtr = body->getWorldTransform();
-    btScalar dst =
-    m_worldInfo->m_sparsesdf.Evaluate(
-                                      wtr.invXform(m_nodes[node].m_x),
-                                      shp,
-                                      nrm,
-                                      0);
-
-    c.m_cti.m_colObj = body;
-    c.m_cti.m_normal = wtr.getBasis() * nrm;
-    c.m_cti.m_offset = dst;
-    c.m_node = &m_nodes[node];
-    const btScalar fc = m_cfg.kDF * body->getFriction();
-    c.m_c2 = ima;
-    c.m_c3 = fc;
-    c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
-    static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
-    const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
-    const btVector3 ra = n.m_x - wtr.getOrigin();
-
-    c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
-    c.m_c1 = ra;
-    c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
-    c.m_node->m_battach = 1;
-    m_deformableAnchors.push_back(c);
+	DeformableNodeRigidAnchor c;
+	btSoftBody::Node& n = m_nodes[node];
+	const btScalar ima = n.m_im;
+	const btScalar imb = body->getInvMass();
+	btVector3 nrm;
+	const btCollisionShape* shp = body->getCollisionShape();
+	const btTransform& wtr = body->getWorldTransform();
+	btScalar dst =
+		m_worldInfo->m_sparsesdf.Evaluate(
+			wtr.invXform(m_nodes[node].m_x),
+			shp,
+			nrm,
+			0);
+
+	c.m_cti.m_colObj = body;
+	c.m_cti.m_normal = wtr.getBasis() * nrm;
+	c.m_cti.m_offset = dst;
+	c.m_node = &m_nodes[node];
+	const btScalar fc = m_cfg.kDF * body->getFriction();
+	c.m_c2 = ima;
+	c.m_c3 = fc;
+	c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
+	static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
+	const btVector3 ra = n.m_x - wtr.getOrigin();
+
+	c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+	c.m_c1 = ra;
+	c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
+	c.m_node->m_battach = 1;
+	m_deformableAnchors.push_back(c);
+}
+
+void btSoftBody::removeAnchor(int node)
+{
+	const btSoftBody::Node& n = m_nodes[node];
+	for (int i = 0; i < m_deformableAnchors.size();)
+	{
+		const DeformableNodeRigidAnchor& c = m_deformableAnchors[i];
+		if (c.m_node == &n)
+		{
+			m_deformableAnchors.removeAtIndex(i);
+		}
+		else
+		{
+			i++;
+		}
+	}
 }
 
 //
 void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
 {
-    DeformableNodeRigidAnchor c;
-    btSoftBody::Node& n = m_nodes[node];
-    const btScalar ima = n.m_im;
-    btVector3 nrm;
-    const btCollisionShape* shp = link->getCollisionShape();
-    const btTransform& wtr = link->getWorldTransform();
-    btScalar dst =
-    m_worldInfo->m_sparsesdf.Evaluate(
-                                      wtr.invXform(m_nodes[node].m_x),
-                                      shp,
-                                      nrm,
-                                      0);
-    c.m_cti.m_colObj = link;
-    c.m_cti.m_normal = wtr.getBasis() * nrm;
-    c.m_cti.m_offset = dst;
-    c.m_node = &m_nodes[node];
-    const btScalar fc = m_cfg.kDF * link->getFriction();
-    c.m_c2 = ima;
-    c.m_c3 = fc;
-    c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
-    btVector3 normal = c.m_cti.m_normal;
-    btVector3 t1 = generateUnitOrthogonalVector(normal);
-    btVector3 t2 = btCross(normal, t1);
-    btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
-    findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
-    findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
-    findJacobian(link, 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 = link->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;
-    const btVector3 ra = n.m_x - wtr.getOrigin();
-    c.m_c1 = ra;
-    c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
-    c.m_node->m_battach = 1;
-    m_deformableAnchors.push_back(c);
+	DeformableNodeRigidAnchor c;
+	btSoftBody::Node& n = m_nodes[node];
+	const btScalar ima = n.m_im;
+	btVector3 nrm;
+	const btCollisionShape* shp = link->getCollisionShape();
+	const btTransform& wtr = link->getWorldTransform();
+	btScalar dst =
+		m_worldInfo->m_sparsesdf.Evaluate(
+			wtr.invXform(m_nodes[node].m_x),
+			shp,
+			nrm,
+			0);
+	c.m_cti.m_colObj = link;
+	c.m_cti.m_normal = wtr.getBasis() * nrm;
+	c.m_cti.m_offset = dst;
+	c.m_node = &m_nodes[node];
+	const btScalar fc = m_cfg.kDF * link->getFriction();
+	c.m_c2 = ima;
+	c.m_c3 = fc;
+	c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
+	btVector3 normal = c.m_cti.m_normal;
+	btVector3 t1 = generateUnitOrthogonalVector(normal);
+	btVector3 t2 = btCross(normal, t1);
+	btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+	findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
+	findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
+	findJacobian(link, 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 = link->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;
+	const btVector3 ra = n.m_x - wtr.getOrigin();
+	c.m_c1 = ra;
+	c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
+	c.m_node->m_battach = 1;
+	m_deformableAnchors.push_back(c);
 }
 //
 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
@@ -731,7 +750,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex
 					fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
 
 					// Check angle of attack
-					// cos(10º) = 0.98480
+					// cos(10º) = 0.98480
 					if (0 < n_dot_v && n_dot_v < 0.98480f)
 						fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
 
@@ -817,7 +836,7 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex
 				fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
 
 				// Check angle of attack
-				// cos(10º) = 0.98480
+				// cos(10º) = 0.98480
 				if (0 < n_dot_v && n_dot_v < 0.98480f)
 					fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
 
@@ -882,6 +901,7 @@ void btSoftBody::setVelocity(const btVector3& velocity)
 		if (n.m_im > 0)
 		{
 			n.m_v = velocity;
+			n.m_vn = velocity;
 		}
 	}
 }
@@ -1010,66 +1030,70 @@ void btSoftBody::setVolumeDensity(btScalar density)
 //
 btVector3 btSoftBody::getLinearVelocity()
 {
-    btVector3 total_momentum = btVector3(0,0,0);
-    for (int i = 0; i < m_nodes.size(); ++i)
-    {
-        btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0/m_nodes[i].m_im;
-        total_momentum += mass * m_nodes[i].m_v;
-    }
-    btScalar total_mass = getTotalMass();
-    return total_mass == 0 ? total_momentum : total_momentum / total_mass;
+	btVector3 total_momentum = btVector3(0, 0, 0);
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0 / m_nodes[i].m_im;
+		total_momentum += mass * m_nodes[i].m_v;
+	}
+	btScalar total_mass = getTotalMass();
+	return total_mass == 0 ? total_momentum : total_momentum / total_mass;
 }
 
 //
 void btSoftBody::setLinearVelocity(const btVector3& linVel)
 {
-    btVector3 old_vel = getLinearVelocity();
-    btVector3 diff = linVel - old_vel;
-    for (int i = 0; i < m_nodes.size(); ++i)
-        m_nodes[i].m_v += diff;
+	btVector3 old_vel = getLinearVelocity();
+	btVector3 diff = linVel - old_vel;
+	for (int i = 0; i < m_nodes.size(); ++i)
+		m_nodes[i].m_v += diff;
 }
 
 //
 void btSoftBody::setAngularVelocity(const btVector3& angVel)
 {
-    btVector3 old_vel = getLinearVelocity();
-    btVector3 com = getCenterOfMass();
-    for (int i = 0; i < m_nodes.size(); ++i)
-    {
-        m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel;
-    }
+	btVector3 old_vel = getLinearVelocity();
+	btVector3 com = getCenterOfMass();
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel;
+	}
 }
 
 //
 btTransform btSoftBody::getRigidTransform()
 {
-    btVector3 t = getCenterOfMass();
-    btMatrix3x3 S;
-    S.setZero();
-    // get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
-    for (int i = 0; i < m_nodes.size(); ++i)
-    {
-        S += OuterProduct(m_X[i], t-m_nodes[i].m_x);
-    }
-    btVector3 sigma;
-    btMatrix3x3 U,V;
-    singularValueDecomposition(S,U,sigma,V);
-    btMatrix3x3 R = V * U.transpose();
-    btTransform trs;
-    trs.setIdentity();
-    trs.setOrigin(t);
-    trs.setBasis(R);
-    return trs;
+	btVector3 t = getCenterOfMass();
+	btMatrix3x3 S;
+	S.setZero();
+	// Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
+	// It's important to make sure that S has the correct signs.
+	// SVD is only unique up to the ordering of singular values.
+	// SVD will manipulate U and V to ensure the ordering of singular values. If all three singular
+	// vaues are negative, SVD will permute colums of U to make two of them positive.
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		S -= OuterProduct(m_X[i], t - m_nodes[i].m_x);
+	}
+	btVector3 sigma;
+	btMatrix3x3 U, V;
+	singularValueDecomposition(S, U, sigma, V);
+	btMatrix3x3 R = V * U.transpose();
+	btTransform trs;
+	trs.setIdentity();
+	trs.setOrigin(t);
+	trs.setBasis(R);
+	return trs;
 }
 
 //
 void btSoftBody::transformTo(const btTransform& trs)
 {
-    // get the current best rigid fit
-    btTransform current_transform = getRigidTransform();
-    // apply transform in material space
-    btTransform new_transform = trs * current_transform.inverse();
-    transform(new_transform);
+	// get the current best rigid fit
+	btTransform current_transform = getRigidTransform();
+	// apply transform in material space
+	btTransform new_transform = trs * current_transform.inverse();
+	transform(new_transform);
 }
 
 //
@@ -1130,7 +1154,7 @@ void btSoftBody::scale(const btVector3& scl)
 	updateNormals();
 	updateBounds();
 	updateConstants();
-    initializeDmInverse();
+	initializeDmInverse();
 }
 
 //
@@ -2010,22 +2034,22 @@ bool btSoftBody::rayTest(const btVector3& rayFrom,
 }
 
 bool btSoftBody::rayFaceTest(const btVector3& rayFrom,
-                         const btVector3& rayTo,
-                         sRayCast& results)
+							 const btVector3& rayTo,
+							 sRayCast& results)
 {
 	if (m_faces.size() == 0)
 		return false;
 	else
 	{
-    	if (m_fdbvt.empty())
-        	initializeFaceTree();
+		if (m_fdbvt.empty())
+			initializeFaceTree();
 	}
-    
-    results.body = this;
-    results.fraction = 1.f;
-    results.index = -1;
-    
-    return (rayFaceTest(rayFrom, rayTo, results.fraction,  results.index) != 0);
+
+	results.body = this;
+	results.fraction = 1.f;
+	results.index = -1;
+
+	return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0);
 }
 
 //
@@ -2056,112 +2080,111 @@ void btSoftBody::setSolver(eSolverPresets::_ preset)
 
 void btSoftBody::predictMotion(btScalar dt)
 {
-    int i, ni;
-    
-    /* Update                */
-    if (m_bUpdateRtCst)
-    {
-        m_bUpdateRtCst = false;
-        updateConstants();
-        m_fdbvt.clear();
-        if (m_cfg.collisions & fCollision::VF_SS)
-        {
-            initializeFaceTree();
-        }
-    }
-    
-    /* Prepare                */
-    m_sst.sdt = dt * m_cfg.timescale;
-    m_sst.isdt = 1 / m_sst.sdt;
-    m_sst.velmrg = m_sst.sdt * 3;
-    m_sst.radmrg = getCollisionShape()->getMargin();
-    m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
-    /* Forces                */
-    addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
-    applyForces();
-    /* Integrate            */
-    for (i = 0, ni = m_nodes.size(); i < ni; ++i)
-    {
-        Node& n = m_nodes[i];
-        n.m_q = n.m_x;
-        btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
-        {
-            btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
-            btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
-            for (int c = 0; c < 3; c++)
-            {
-                if (deltaV[c] > clampDeltaV)
-                {
-                    deltaV[c] = clampDeltaV;
-                }
-                if (deltaV[c] < -clampDeltaV)
-                {
-                    deltaV[c] = -clampDeltaV;
-                }
-            }
-        }
-        n.m_v += deltaV;
-        n.m_x += n.m_v * m_sst.sdt;
-        n.m_f = btVector3(0, 0, 0);
-    }
-    /* Clusters                */
-    updateClusters();
-    /* Bounds                */
-    updateBounds();
-    /* Nodes                */
-    ATTRIBUTE_ALIGNED16(btDbvtVolume)
-    vol;
-    for (i = 0, ni = m_nodes.size(); i < ni; ++i)
-    {
-        Node& n = m_nodes[i];
-        vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
-        m_ndbvt.update(n.m_leaf,
-                       vol,
-                       n.m_v * m_sst.velmrg,
-                       m_sst.updmrg);
-    }
-    /* Faces                */
-    if (!m_fdbvt.empty())
-    {
-        for (int i = 0; i < m_faces.size(); ++i)
-        {
-            Face& f = m_faces[i];
-            const btVector3 v = (f.m_n[0]->m_v +
-                                 f.m_n[1]->m_v +
-                                 f.m_n[2]->m_v) /
-            3;
-            vol = VolumeOf(f, m_sst.radmrg);
-            m_fdbvt.update(f.m_leaf,
-                           vol,
-                           v * m_sst.velmrg,
-                           m_sst.updmrg);
-        }
-    }
-    /* Pose                    */
-    updatePose();
-    /* Match                */
-    if (m_pose.m_bframe && (m_cfg.kMT > 0))
-    {
-        const btMatrix3x3 posetrs = m_pose.m_rot;
-        for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
-        {
-            Node& n = m_nodes[i];
-            if (n.m_im > 0)
-            {
-                const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
-                n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
-            }
-        }
-    }
-    /* Clear contacts        */
-    m_rcontacts.resize(0);
-    m_scontacts.resize(0);
-    /* Optimize dbvt's        */
-    m_ndbvt.optimizeIncremental(1);
-    m_fdbvt.optimizeIncremental(1);
-    m_cdbvt.optimizeIncremental(1);
-}
+	int i, ni;
 
+	/* Update                */
+	if (m_bUpdateRtCst)
+	{
+		m_bUpdateRtCst = false;
+		updateConstants();
+		m_fdbvt.clear();
+		if (m_cfg.collisions & fCollision::VF_SS)
+		{
+			initializeFaceTree();
+		}
+	}
+
+	/* Prepare                */
+	m_sst.sdt = dt * m_cfg.timescale;
+	m_sst.isdt = 1 / m_sst.sdt;
+	m_sst.velmrg = m_sst.sdt * 3;
+	m_sst.radmrg = getCollisionShape()->getMargin();
+	m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
+	/* Forces                */
+	addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
+	applyForces();
+	/* Integrate            */
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
+	{
+		Node& n = m_nodes[i];
+		n.m_q = n.m_x;
+		btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
+		{
+			btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
+			btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
+			for (int c = 0; c < 3; c++)
+			{
+				if (deltaV[c] > clampDeltaV)
+				{
+					deltaV[c] = clampDeltaV;
+				}
+				if (deltaV[c] < -clampDeltaV)
+				{
+					deltaV[c] = -clampDeltaV;
+				}
+			}
+		}
+		n.m_v += deltaV;
+		n.m_x += n.m_v * m_sst.sdt;
+		n.m_f = btVector3(0, 0, 0);
+	}
+	/* Clusters                */
+	updateClusters();
+	/* Bounds                */
+	updateBounds();
+	/* Nodes                */
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)
+	vol;
+	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
+	{
+		Node& n = m_nodes[i];
+		vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
+		m_ndbvt.update(n.m_leaf,
+					   vol,
+					   n.m_v * m_sst.velmrg,
+					   m_sst.updmrg);
+	}
+	/* Faces                */
+	if (!m_fdbvt.empty())
+	{
+		for (int i = 0; i < m_faces.size(); ++i)
+		{
+			Face& f = m_faces[i];
+			const btVector3 v = (f.m_n[0]->m_v +
+								 f.m_n[1]->m_v +
+								 f.m_n[2]->m_v) /
+								3;
+			vol = VolumeOf(f, m_sst.radmrg);
+			m_fdbvt.update(f.m_leaf,
+						   vol,
+						   v * m_sst.velmrg,
+						   m_sst.updmrg);
+		}
+	}
+	/* Pose                    */
+	updatePose();
+	/* Match                */
+	if (m_pose.m_bframe && (m_cfg.kMT > 0))
+	{
+		const btMatrix3x3 posetrs = m_pose.m_rot;
+		for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
+		{
+			Node& n = m_nodes[i];
+			if (n.m_im > 0)
+			{
+				const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
+				n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
+			}
+		}
+	}
+	/* Clear contacts        */
+	m_rcontacts.resize(0);
+	m_scontacts.resize(0);
+	/* Optimize dbvt's        */
+	m_ndbvt.optimizeIncremental(1);
+	m_fdbvt.optimizeIncremental(1);
+	m_cdbvt.optimizeIncremental(1);
+}
 
 //
 void btSoftBody::solveConstraints()
@@ -2534,12 +2557,12 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
 }
 
 int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
-						btScalar& mint, int& index) const
+							btScalar& mint, int& index) const
 {
 	int cnt = 0;
 	{ /* Use dbvt	*/
 		RayFromToCaster collider(rayFrom, rayTo, mint);
-		
+
 		btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
 		if (collider.m_face)
 		{
@@ -2551,7 +2574,6 @@ int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
 	return (cnt);
 }
 
-
 //
 static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
 {
@@ -2580,7 +2602,7 @@ static inline void calculateNormalCone(btDbvntNode* root)
 	}
 	else
 	{
-		btVector3 n0(0,0,0), n1(0,0,0);
+		btVector3 n0(0, 0, 0), n1(0, 0, 0);
 		btScalar a0 = 0, a1 = 0;
 		if (root->childs[0])
 		{
@@ -2594,8 +2616,8 @@ static inline void calculateNormalCone(btDbvntNode* root)
 			n1 = root->childs[1]->normal;
 			a1 = root->childs[1]->angle;
 		}
-		root->normal = (n0+n1).safeNormalize();
-		root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
+		root->normal = (n0 + n1).safeNormalize();
+		root->angle = btMax(a0, a1) + btAngle(n0, n1) * 0.5;
 	}
 }
 
@@ -2609,7 +2631,8 @@ void btSoftBody::initializeFaceTree()
 	for (int i = 0; i < m_faces.size(); ++i)
 	{
 		Face& f = m_faces[i];
-		ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0);
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		vol = VolumeOf(f, 0);
 		btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
 		node->parent = NULL;
 		node->data = &f;
@@ -2623,7 +2646,7 @@ void btSoftBody::initializeFaceTree()
 	// construct the adjacency list for triangles
 	for (int i = 0; i < adj.size(); ++i)
 	{
-		for (int j = i+1; j < adj.size(); ++j)
+		for (int j = i + 1; j < adj.size(); ++j)
 		{
 			int dup = 0;
 			for (int k = 0; k < 3; ++k)
@@ -2661,7 +2684,8 @@ void btSoftBody::rebuildNodeTree()
 	for (int i = 0; i < m_nodes.size(); ++i)
 	{
 		Node& n = m_nodes[i];
-		ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0);
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)
+		vol = btDbvtVolume::FromCR(n.m_x, 0);
 		btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
 		node->parent = NULL;
 		node->data = &n;
@@ -2704,61 +2728,61 @@ btVector3 btSoftBody::evaluateCom() const
 }
 
 bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
-                              const btVector3& x,
-                              btScalar margin,
-                              btSoftBody::sCti& cti) const
-{
-    btVector3 nrm;
-    const btCollisionShape* shp = colObjWrap->getCollisionShape();
-    //    const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
-    //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
-    const btTransform& wtr = colObjWrap->getWorldTransform();
-    //todo: check which transform is needed here
-    
-    btScalar dst =
-    m_worldInfo->m_sparsesdf.Evaluate(
-                                      wtr.invXform(x),
-                                      shp,
-                                      nrm,
-                                      margin);
-    if (dst < 0)
-    {
-        cti.m_colObj = colObjWrap->getCollisionObject();
-        cti.m_normal = wtr.getBasis() * nrm;
-        cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
-        return (true);
-    }
-    return (false);
+							  const btVector3& x,
+							  btScalar margin,
+							  btSoftBody::sCti& cti) const
+{
+	btVector3 nrm;
+	const btCollisionShape* shp = colObjWrap->getCollisionShape();
+	//    const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
+	//const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
+	const btTransform& wtr = colObjWrap->getWorldTransform();
+	//todo: check which transform is needed here
+
+	btScalar dst =
+		m_worldInfo->m_sparsesdf.Evaluate(
+			wtr.invXform(x),
+			shp,
+			nrm,
+			margin);
+	if (dst < 0)
+	{
+		cti.m_colObj = colObjWrap->getCollisionObject();
+		cti.m_normal = wtr.getBasis() * nrm;
+		cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
+		return (true);
+	}
+	return (false);
 }
 
 //
 bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap,
-							  const btVector3& x,
-							  btScalar margin,
-							  btSoftBody::sCti& cti, bool predict) const
+										const btVector3& x,
+										btScalar margin,
+										btSoftBody::sCti& cti, bool predict) const
 {
 	btVector3 nrm;
 	const btCollisionShape* shp = colObjWrap->getCollisionShape();
-    const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
-    // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
-    // but resolve contact at x_n
-    btTransform wtr = (predict) ?
-    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
-                 : colObjWrap->getWorldTransform();
+	const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
+	// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
+	// but resolve contact at x_n
+	btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+								: colObjWrap->getWorldTransform();
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
 			wtr.invXform(x),
 			shp,
 			nrm,
 			margin);
+
 	if (!predict)
 	{
 		cti.m_colObj = colObjWrap->getCollisionObject();
 		cti.m_normal = wtr.getBasis() * nrm;
-        cti.m_offset = dst;
+		cti.m_offset = dst;
 	}
-    if (dst < 0)
-        return true;
+	if (dst < 0)
+		return true;
 	return (false);
 }
 
@@ -2767,175 +2791,131 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
 // point p with respect to triangle (a, b, c)
 static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary)
 {
-    btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
-    btScalar d00 = v0.dot(v0);
-    btScalar d01 = v0.dot(v1);
-    btScalar d11 = v1.dot(v1);
-    btScalar d20 = v2.dot(v0);
-    btScalar d21 = v2.dot(v1);
-    btScalar denom = d00 * d11 - d01 * d01;
-    bary.setY((d11 * d20 - d01 * d21) / denom);
-    bary.setZ((d00 * d21 - d01 * d20) / denom);
-    bary.setX(btScalar(1) - bary.getY() - bary.getZ());
+	btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
+	btScalar d00 = v0.dot(v0);
+	btScalar d01 = v0.dot(v1);
+	btScalar d11 = v1.dot(v1);
+	btScalar d20 = v2.dot(v0);
+	btScalar d21 = v2.dot(v1);
+	btScalar denom = d00 * d11 - d01 * d01;
+	bary.setY((d11 * d20 - d01 * d21) / denom);
+	bary.setZ((d00 * d21 - d01 * d20) / denom);
+	bary.setX(btScalar(1) - bary.getY() - bary.getZ());
 }
 
 //
 bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap,
-                                        Face& f,
-                                        btVector3& contact_point,
-                                        btVector3& bary,
-                                        btScalar margin,
-                                        btSoftBody::sCti& cti, bool predict) const
-{
-    btVector3 nrm;
-    const btCollisionShape* shp = colObjWrap->getCollisionShape();
-    const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
-    // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
-    // but resolve contact at x_n
-    btTransform wtr = (predict) ?
-    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
-    : colObjWrap->getWorldTransform();
-    btScalar dst;
-    
-//#define USE_QUADRATURE 1
-//#define CACHE_PREV_COLLISION
-    
-    // use the contact position of the previous collision
-#ifdef CACHE_PREV_COLLISION
-    if (f.m_pcontact[3] != 0)
-    {
-        for (int i = 0; i < 3; ++i)
-            bary[i] = f.m_pcontact[i];
-        contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-        dst = m_worldInfo->m_sparsesdf.Evaluate(
-                                          wtr.invXform(contact_point),
-                                          shp,
-                                          nrm,
-                                          margin);
-        nrm = wtr.getBasis() * nrm;
-        cti.m_colObj = colObjWrap->getCollisionObject();
-        // use cached contact point
-    }
-    else
-    {
-        btGjkEpaSolver2::sResults results;
-        btTransform triangle_transform;
-        triangle_transform.setIdentity();
-        triangle_transform.setOrigin(f.m_n[0]->m_x);
-        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
-        btVector3 guess(0,0,0);
-        const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
-        btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
-        dst = results.distance - margin;
-        contact_point = results.witnesses[0];
-        getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-        nrm = results.normal;
-        cti.m_colObj = colObjWrap->getCollisionObject();
-        for (int i = 0; i < 3; ++i)
-            f.m_pcontact[i] = bary[i];
-    }
-    return (dst < 0);
-#endif
+											Face& f,
+											btVector3& contact_point,
+											btVector3& bary,
+											btScalar margin,
+											btSoftBody::sCti& cti, bool predict) const
+{
+	btVector3 nrm;
+	const btCollisionShape* shp = colObjWrap->getCollisionShape();
+	const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
+	// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
+	// but resolve contact at x_n
+	btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+								: colObjWrap->getWorldTransform();
+	btScalar dst;
+	btGjkEpaSolver2::sResults results;
+
+//	#define USE_QUADRATURE 1
 
-    // use collision quadrature point
+	// use collision quadrature point
 #ifdef USE_QUADRATURE
-    {
-        dst = SIMD_INFINITY;
-        btVector3 local_nrm;
-        for (int q = 0; q < m_quads.size(); ++q)
-        {
-            btVector3 p;
-            if (predict)
-                p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
-            else
-                p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
-            btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
-                                                    wtr.invXform(p),
-                                                    shp,
-                                                    local_nrm,
-                                                    margin);
-            if (local_dst < dst)
-            {
-                if (local_dst < 0 && predict)
-                    return true;
-                dst = local_dst;
-                contact_point = p;
-                bary = m_quads[q];
-                nrm = local_nrm;
-            }
-            if (!predict)
-            {
-                cti.m_colObj = colObjWrap->getCollisionObject();
-                cti.m_normal = wtr.getBasis() * nrm;
-                cti.m_offset = dst;
-            }
-        }
-        return (dst < 0);
-    }
+	{
+		dst = SIMD_INFINITY;
+		btVector3 local_nrm;
+		for (int q = 0; q < m_quads.size(); ++q)
+		{
+			btVector3 p;
+			if (predict)
+				p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
+			else
+				p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
+			btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
+				wtr.invXform(p),
+				shp,
+				local_nrm,
+				margin);
+			if (local_dst < dst)
+			{
+				if (local_dst < 0 && predict)
+					return true;
+				dst = local_dst;
+				contact_point = p;
+				bary = m_quads[q];
+				nrm = local_nrm;
+			}
+			if (!predict)
+			{
+				cti.m_colObj = colObjWrap->getCollisionObject();
+				cti.m_normal = wtr.getBasis() * nrm;
+				cti.m_offset = dst;
+			}
+		}
+		return (dst < 0);
+	}
 #endif
-    
-//    // regular face contact
-//    {
-//        btGjkEpaSolver2::sResults results;
-//        btTransform triangle_transform;
-//        triangle_transform.setIdentity();
-//        triangle_transform.setOrigin(f.m_n[0]->m_x);
-//        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
-//        btVector3 guess(0,0,0);
-//        if (predict)
-//        {
-//            triangle_transform.setOrigin(f.m_n[0]->m_q);
-//            triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
-//        }
-//        const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
-////        btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
-////        dst = results.distance - margin;
-////        contact_point = results.witnesses[0];
-//        btGjkEpaSolver2::Penetration(&triangle, triangle_transform, csh, wtr, guess, results);
-//        if (results.status == btGjkEpaSolver2::sResults::Separated)
-//            return false;
-//        dst = results.distance - margin;
-//        contact_point = results.witnesses[1];
-//        getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-//        nrm = results.normal;
-//        for (int i = 0; i < 3; ++i)
-//            f.m_pcontact[i] = bary[i];
-//    }
-//
-//    if (!predict)
-//    {
-//        cti.m_colObj = colObjWrap->getCollisionObject();
-//        cti.m_normal = nrm;
-//        cti.m_offset = dst;
-//    }
-//
-    
-    // regular face contact
-    {
-        btGjkEpaSolver2::sResults results;
-        btTransform triangle_transform;
-        triangle_transform.setIdentity();
-        triangle_transform.setOrigin(f.m_n[0]->m_q);
-        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
-        btVector3 guess(0,0,0);
-        const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
-        btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
-        dst = results.distance-csh->getMargin();
-        dst -= margin;
-        if (dst >= 0)
-            return false;
-        contact_point = results.witnesses[0];
-        getBarycentric(contact_point, f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, bary);
-        btVector3 curr = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-        nrm = results.normal;
-        cti.m_colObj = colObjWrap->getCollisionObject();
-        cti.m_normal = nrm;
-        cti.m_offset = dst + (curr - contact_point).dot(nrm);
-    }
-    return (dst < 0);
+
+	// collision detection using x*
+	btTransform triangle_transform;
+	triangle_transform.setIdentity();
+	triangle_transform.setOrigin(f.m_n[0]->m_q);
+	btTriangleShape triangle(btVector3(0, 0, 0), f.m_n[1]->m_q - f.m_n[0]->m_q, f.m_n[2]->m_q - f.m_n[0]->m_q);
+	btVector3 guess(0, 0, 0);
+	const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+	btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
+	dst = results.distance - 2.0 * csh->getMargin() - margin;  // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable
+	if (dst >= 0)
+		return false;
+
+	// Use consistent barycenter to recalculate distance.
+	if (this->m_cacheBarycenter)
+	{
+		if (f.m_pcontact[3] != 0)
+		{
+			for (int i = 0; i < 3; ++i)
+				bary[i] = f.m_pcontact[i];
+			contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+			const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+			btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results);
+			cti.m_colObj = colObjWrap->getCollisionObject();
+			dst = results.distance;
+			cti.m_normal = results.normal;
+			cti.m_offset = dst;
+
+			//point-convex CD
+			wtr = colObjWrap->getWorldTransform();
+			btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
+			triangle_transform.setOrigin(f.m_n[0]->m_x);
+			btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
+
+			dst = results.distance - csh->getMargin() - margin;
+			return true;
+		}
+	}
+
+	// Use triangle-convex CD.
+	wtr = colObjWrap->getWorldTransform();
+	btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x);
+	triangle_transform.setOrigin(f.m_n[0]->m_x);
+	btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
+	contact_point = results.witnesses[0];
+	getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+
+	for (int i = 0; i < 3; ++i)
+		f.m_pcontact[i] = bary[i];
+
+	dst = results.distance - csh->getMargin() - margin;
+	cti.m_colObj = colObjWrap->getCollisionObject();
+	cti.m_normal = results.normal;
+	cti.m_offset = dst;
+	return true;
 }
 
-//
 void btSoftBody::updateNormals()
 {
 	const btVector3 zv(0, 0, 0);
@@ -2979,63 +2959,63 @@ void btSoftBody::updateBounds()
 		m_bounds[1] = btVector3(1000, 1000, 1000);
 
 	} else {*/
-//    if (m_ndbvt.m_root)
-//    {
-//        const btVector3& mins = m_ndbvt.m_root->volume.Mins();
-//        const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
-//        const btScalar csm = getCollisionShape()->getMargin();
-//        const btVector3 mrg = btVector3(csm,
-//                                        csm,
-//                                        csm) *
-//                              1;  // ??? to investigate...
-//        m_bounds[0] = mins - mrg;
-//        m_bounds[1] = maxs + mrg;
-//        if (0 != getBroadphaseHandle())
-//        {
-//            m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
-//                                               m_bounds[0],
-//                                               m_bounds[1],
-//                                               m_worldInfo->m_dispatcher);
-//        }
-//    }
-//    else
-//    {
-//        m_bounds[0] =
-//            m_bounds[1] = btVector3(0, 0, 0);
-//    }
-    if (m_nodes.size())
-    {
-        btVector3 mins = m_nodes[0].m_x;
-        btVector3 maxs = m_nodes[0].m_x;
-        for (int i = 1; i < m_nodes.size(); ++i)
-        {
-            for (int d = 0; d < 3; ++d)
-            {
-                if (m_nodes[i].m_x[d] > maxs[d])
-                    maxs[d] = m_nodes[i].m_x[d];
-                if (m_nodes[i].m_x[d] < mins[d])
-                    mins[d] = m_nodes[i].m_x[d];
-            }
-        }
-        const btScalar csm = getCollisionShape()->getMargin();
-        const btVector3 mrg = btVector3(csm,
-                                        csm,
-                                        csm);
-        m_bounds[0] = mins - mrg;
-        m_bounds[1] = maxs + mrg;
-        if (0 != getBroadphaseHandle())
-        {
-            m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
-                                               m_bounds[0],
-                                               m_bounds[1],
-                                               m_worldInfo->m_dispatcher);
-        }
-    }
-    else
-    {
-        m_bounds[0] =
-        m_bounds[1] = btVector3(0, 0, 0);
-    }
+	//    if (m_ndbvt.m_root)
+	//    {
+	//        const btVector3& mins = m_ndbvt.m_root->volume.Mins();
+	//        const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
+	//        const btScalar csm = getCollisionShape()->getMargin();
+	//        const btVector3 mrg = btVector3(csm,
+	//                                        csm,
+	//                                        csm) *
+	//                              1;  // ??? to investigate...
+	//        m_bounds[0] = mins - mrg;
+	//        m_bounds[1] = maxs + mrg;
+	//        if (0 != getBroadphaseHandle())
+	//        {
+	//            m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
+	//                                               m_bounds[0],
+	//                                               m_bounds[1],
+	//                                               m_worldInfo->m_dispatcher);
+	//        }
+	//    }
+	//    else
+	//    {
+	//        m_bounds[0] =
+	//            m_bounds[1] = btVector3(0, 0, 0);
+	//    }
+	if (m_nodes.size())
+	{
+		btVector3 mins = m_nodes[0].m_x;
+		btVector3 maxs = m_nodes[0].m_x;
+		for (int i = 1; i < m_nodes.size(); ++i)
+		{
+			for (int d = 0; d < 3; ++d)
+			{
+				if (m_nodes[i].m_x[d] > maxs[d])
+					maxs[d] = m_nodes[i].m_x[d];
+				if (m_nodes[i].m_x[d] < mins[d])
+					mins[d] = m_nodes[i].m_x[d];
+			}
+		}
+		const btScalar csm = getCollisionShape()->getMargin();
+		const btVector3 mrg = btVector3(csm,
+										csm,
+										csm);
+		m_bounds[0] = mins - mrg;
+		m_bounds[1] = maxs + mrg;
+		if (0 != getBroadphaseHandle())
+		{
+			m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
+											   m_bounds[0],
+											   m_bounds[1],
+											   m_worldInfo->m_dispatcher);
+		}
+	}
+	else
+	{
+		m_bounds[0] =
+			m_bounds[1] = btVector3(0, 0, 0);
+	}
 }
 
 //
@@ -3454,60 +3434,120 @@ void btSoftBody::dampClusters()
 
 void btSoftBody::setSpringStiffness(btScalar k)
 {
-    for (int i = 0; i < m_links.size(); ++i)
-    {
-        m_links[i].Feature::m_material->m_kLST = k;
-    }
-    m_repulsionStiffness = k;
+	for (int i = 0; i < m_links.size(); ++i)
+	{
+		m_links[i].Feature::m_material->m_kLST = k;
+	}
+	m_repulsionStiffness = k;
+}
+
+void btSoftBody::setGravityFactor(btScalar gravFactor)
+{
+	m_gravityFactor = gravFactor;
+}
+
+void btSoftBody::setCacheBarycenter(bool cacheBarycenter)
+{
+	m_cacheBarycenter = cacheBarycenter;
 }
 
 void btSoftBody::initializeDmInverse()
 {
-    btScalar unit_simplex_measure = 1./6.;
-    
-    for (int i = 0; i < m_tetras.size(); ++i)
-    {
-        Tetra &t = m_tetras[i];
-        btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
-        btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
-        btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
-        btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
-                       c1.getY(), c2.getY(), c3.getY(),
-                       c1.getZ(), c2.getZ(), c3.getZ());
-        t.m_element_measure = Dm.determinant() * unit_simplex_measure;
-        t.m_Dm_inverse = Dm.inverse();
-    }
+	btScalar unit_simplex_measure = 1. / 6.;
+
+	for (int i = 0; i < m_tetras.size(); ++i)
+	{
+		Tetra& t = m_tetras[i];
+		btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
+		btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
+		btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
+		btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
+					   c1.getY(), c2.getY(), c3.getY(),
+					   c1.getZ(), c2.getZ(), c3.getZ());
+		t.m_element_measure = Dm.determinant() * unit_simplex_measure;
+		t.m_Dm_inverse = Dm.inverse();
+
+		// calculate the first three columns of P^{-1}
+		btVector3 a = t.m_n[0]->m_x;
+		btVector3 b = t.m_n[1]->m_x;
+		btVector3 c = t.m_n[2]->m_x;
+		btVector3 d = t.m_n[3]->m_x;
+
+		btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]);
+
+		btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2];
+		btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2];
+		btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1];
+		btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2];
+		btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2];
+		btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1];
+		btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2];
+		btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2];
+		btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1];
+		btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2];
+		btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2];
+		btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1];
+
+		btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det);
+		btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det);
+		btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det);
+
+		t.m_P_inv[0] = p1;
+		t.m_P_inv[1] = p2;
+		t.m_P_inv[2] = p3;
+	}
+}
+
+static btScalar Dot4(const btVector4& a, const btVector4& b)
+{
+	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
 }
 
 void btSoftBody::updateDeformation()
 {
-    for (int i = 0; i < m_tetras.size(); ++i)
-    {
-        btSoftBody::Tetra& t = m_tetras[i];
-        btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
-        btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
-        btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
-        btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
-                       c1.getY(), c2.getY(), c3.getY(),
-                       c1.getZ(), c2.getZ(), c3.getZ());
-        t.m_F = Ds * t.m_Dm_inverse;
-        
-        btSoftBody::TetraScratch& s = m_tetraScratches[i];
-        s.m_F = t.m_F;
-        s.m_J = t.m_F.determinant();
-        btMatrix3x3 C = t.m_F.transpose()*t.m_F;
-        s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
-        s.m_cofF = t.m_F.adjoint().transpose();
-    }
+	btQuaternion q;
+	for (int i = 0; i < m_tetras.size(); ++i)
+	{
+		btSoftBody::Tetra& t = m_tetras[i];
+		btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
+		btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
+		btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
+		btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
+					   c1.getY(), c2.getY(), c3.getY(),
+					   c1.getZ(), c2.getZ(), c3.getZ());
+		t.m_F = Ds * t.m_Dm_inverse;
+
+		btSoftBody::TetraScratch& s = m_tetraScratches[i];
+		s.m_F = t.m_F;
+		s.m_J = t.m_F.determinant();
+		btMatrix3x3 C = t.m_F.transpose() * t.m_F;
+		s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
+		s.m_cofF = t.m_F.adjoint().transpose();
+
+		btVector3 a = t.m_n[0]->m_q;
+		btVector3 b = t.m_n[1]->m_q;
+		btVector3 c = t.m_n[2]->m_q;
+		btVector3 d = t.m_n[3]->m_q;
+		btVector4 q1(a[0], b[0], c[0], d[0]);
+		btVector4 q2(a[1], b[1], c[1], d[1]);
+		btVector4 q3(a[2], b[2], c[2], d[2]);
+		btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]),
+					  Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]),
+					  Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2]));
+		q.setRotation(btVector3(0, 0, 1), 0);
+		B.extractRotation(q, 0.01);  // precision of the rotation is not very important for visual correctness.
+		btMatrix3x3 Q(q);
+		s.m_corotation = Q;
+	}
 }
 
 void btSoftBody::advanceDeformation()
 {
-    updateDeformation();
-    for (int i = 0; i < m_tetras.size(); ++i)
-    {
-        m_tetraScratchesTn[i] = m_tetraScratches[i];
-    }
+	updateDeformation();
+	for (int i = 0; i < m_tetras.size(); ++i)
+	{
+		m_tetraScratchesTn[i] = m_tetraScratches[i];
+	}
 }
 //
 void btSoftBody::Joint::Prepare(btScalar dt, int)
@@ -3750,7 +3790,7 @@ void btSoftBody::applyForces()
 //
 void btSoftBody::setMaxStress(btScalar maxStress)
 {
-    m_cfg.m_maxStress = maxStress;
+	m_cfg.m_maxStress = maxStress;
 }
 
 //
@@ -3765,7 +3805,7 @@ void btSoftBody::interpolateRenderMesh()
 			const Node* p2 = m_renderNodesParents[i][2];
 			btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x);
 			btVector3 unit_normal = normal.normalized();
-			Node& n = m_renderNodes[i];
+			RenderNode& n = m_renderNodes[i];
 			n.m_x.setZero();
 			for (int j = 0; j < 3; ++j)
 			{
@@ -3778,7 +3818,7 @@ void btSoftBody::interpolateRenderMesh()
 	{
 		for (int i = 0; i < m_renderNodes.size(); ++i)
 		{
-			Node& n = m_renderNodes[i];
+			RenderNode& n = m_renderNodes[i];
 			n.m_x.setZero();
 			for (int j = 0; j < 4; ++j)
 			{
@@ -3793,13 +3833,13 @@ void btSoftBody::interpolateRenderMesh()
 
 void btSoftBody::setCollisionQuadrature(int N)
 {
-    for (int i = 0; i <= N; ++i)
-    {
-        for (int j = 0; i+j <= N; ++j)
-        {
-            m_quads.push_back(btVector3(btScalar(i)/btScalar(N), btScalar(j)/btScalar(N), btScalar(N-i-j)/btScalar(N)));
-        }
-    }
+	for (int i = 0; i <= N; ++i)
+	{
+		for (int j = 0; i + j <= N; ++j)
+		{
+			m_quads.push_back(btVector3(btScalar(i) / btScalar(N), btScalar(j) / btScalar(N), btScalar(N - i - j) / btScalar(N)));
+		}
+	}
 }
 
 //
@@ -4006,12 +4046,12 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
 
 void btSoftBody::setSelfCollision(bool useSelfCollision)
 {
-    m_useSelfCollision = useSelfCollision;
+	m_useSelfCollision = useSelfCollision;
 }
 
 bool btSoftBody::useSelfCollision()
 {
-   return m_useSelfCollision;
+	return m_useSelfCollision;
 }
 
 //
@@ -4052,23 +4092,23 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 			collider.ProcessColObj(this, pcoWrap);
 		}
 		break;
-        case fCollision::SDF_RD:
-        {
-            btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
-            if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
-            {
-                const btTransform wtr = pcoWrap->getWorldTransform();
-                const btScalar timemargin = 0;
-                const btScalar basemargin = getCollisionShape()->getMargin();
-                btVector3 mins;
-                btVector3 maxs;
-                ATTRIBUTE_ALIGNED16(btDbvtVolume)
-                volume;
-                pcoWrap->getCollisionShape()->getAabb(wtr,
-                                                      mins,
-                                                      maxs);
-                volume = btDbvtVolume::FromMM(mins, maxs);
-                volume.Expand(btVector3(basemargin, basemargin, basemargin));
+		case fCollision::SDF_RD:
+		{
+			btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
+			if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
+			{
+				const btTransform wtr = pcoWrap->getWorldTransform();
+				const btScalar timemargin = 0;
+				const btScalar basemargin = getCollisionShape()->getMargin();
+				btVector3 mins;
+				btVector3 maxs;
+				ATTRIBUTE_ALIGNED16(btDbvtVolume)
+				volume;
+				pcoWrap->getCollisionShape()->getAabb(wtr,
+													  mins,
+													  maxs);
+				volume = btDbvtVolume::FromMM(mins, maxs);
+				volume.Expand(btVector3(basemargin, basemargin, basemargin));
 				if (m_cfg.collisions & fCollision::SDF_RDN)
 				{
 					btSoftColliders::CollideSDF_RD docollideNode;
@@ -4080,26 +4120,26 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 					m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
 				}
 
-                if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF)))
-                {
-                    btSoftColliders::CollideSDF_RDF docollideFace;
-                    docollideFace.psb = this;
-                    docollideFace.m_colObj1Wrap = pcoWrap;
-                    docollideFace.m_rigidBody = prb1;
+				if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF)))
+				{
+					btSoftColliders::CollideSDF_RDF docollideFace;
+					docollideFace.psb = this;
+					docollideFace.m_colObj1Wrap = pcoWrap;
+					docollideFace.m_rigidBody = prb1;
 					docollideFace.dynmargin = basemargin + timemargin;
 					docollideFace.stamargin = basemargin;
-                    m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
-                }
-            }
-        }
-        break;
+					m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
+				}
+			}
+		}
+		break;
 	}
 }
 
 //
 void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 {
-    BT_PROFILE("Deformable Collision");
+	BT_PROFILE("Deformable Collision");
 	const int cf = m_cfg.collisions & psb->m_cfg.collisions;
 	switch (cf & fCollision::SVSmask)
 	{
@@ -4137,60 +4177,60 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 			}
 		}
 		break;
-        case fCollision::VF_DD:
-        {
-            if (!psb->m_softSoftCollision)
-                return;
-            if (psb->isActive() || this->isActive())
-            {
-                if (this != psb)
-                {
-                    btSoftColliders::CollideVF_DD docollide;
-                    /* common                    */
-                    docollide.mrg = getCollisionShape()->getMargin() +
-                    psb->getCollisionShape()->getMargin();
-                    /* psb0 nodes vs psb1 faces    */
-                    if (psb->m_tetras.size() > 0)
-                        docollide.useFaceNormal = true;
-                    else
-                        docollide.useFaceNormal = false;
-                    docollide.psb[0] = this;
-                    docollide.psb[1] = psb;
-                    docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
-                                                        docollide.psb[1]->m_fdbvt.m_root,
-                                                        docollide);
-
-                    /* psb1 nodes vs psb0 faces    */
-                    if (this->m_tetras.size() > 0)
-                        docollide.useFaceNormal = true;
-                    else
-                        docollide.useFaceNormal = false;
-                    docollide.psb[0] = psb;
-                    docollide.psb[1] = this;
-                    docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
-                                                        docollide.psb[1]->m_fdbvt.m_root,
-                                                        docollide);
-                }
-                else
-                {
-                    if (psb->useSelfCollision())
-                    {
-                        btSoftColliders::CollideFF_DD docollide;
-                        docollide.mrg = 2*getCollisionShape()->getMargin();
-                        docollide.psb[0] = this;
-                        docollide.psb[1] = psb;
-                        if (this->m_tetras.size() > 0)
-                            docollide.useFaceNormal = true;
-                        else
-                            docollide.useFaceNormal = false;
-                        /* psb0 faces vs psb0 faces    */
-                        calculateNormalCone(this->m_fdbvnt);
-                        this->m_fdbvt.selfCollideT(m_fdbvnt,docollide);
-                    }
-                }
-            }
-        }
-        break;
+		case fCollision::VF_DD:
+		{
+			if (!psb->m_softSoftCollision)
+				return;
+			if (psb->isActive() || this->isActive())
+			{
+				if (this != psb)
+				{
+					btSoftColliders::CollideVF_DD docollide;
+					/* common                    */
+					docollide.mrg = getCollisionShape()->getMargin() +
+									psb->getCollisionShape()->getMargin();
+					/* psb0 nodes vs psb1 faces    */
+					if (psb->m_tetras.size() > 0)
+						docollide.useFaceNormal = true;
+					else
+						docollide.useFaceNormal = false;
+					docollide.psb[0] = this;
+					docollide.psb[1] = psb;
+					docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+														docollide.psb[1]->m_fdbvt.m_root,
+														docollide);
+
+					/* psb1 nodes vs psb0 faces    */
+					if (this->m_tetras.size() > 0)
+						docollide.useFaceNormal = true;
+					else
+						docollide.useFaceNormal = false;
+					docollide.psb[0] = psb;
+					docollide.psb[1] = this;
+					docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+														docollide.psb[1]->m_fdbvt.m_root,
+														docollide);
+				}
+				else
+				{
+					if (psb->useSelfCollision())
+					{
+						btSoftColliders::CollideFF_DD docollide;
+						docollide.mrg = 2 * getCollisionShape()->getMargin();
+						docollide.psb[0] = this;
+						docollide.psb[1] = psb;
+						if (this->m_tetras.size() > 0)
+							docollide.useFaceNormal = true;
+						else
+							docollide.useFaceNormal = false;
+						/* psb0 faces vs psb0 faces    */
+						calculateNormalCone(this->m_fdbvnt);
+						this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
+					}
+				}
+			}
+		}
+		break;
 		default:
 		{
 		}
@@ -4205,7 +4245,7 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
 		{
 			btSoftColliders::CollideCCD docollide;
 			/* common                    */
-			docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin
+			docollide.mrg = SAFE_EPSILON;  // for rounding error instead of actual margin
 			docollide.dt = psb->m_sst.sdt;
 			/* psb0 nodes vs psb1 faces    */
 			if (psb->m_tetras.size() > 0)
@@ -4215,8 +4255,8 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
 			docollide.psb[0] = this;
 			docollide.psb[1] = psb;
 			docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
-												 docollide.psb[1]->m_fdbvt.m_root,
-												 docollide);
+												docollide.psb[1]->m_fdbvt.m_root,
+												docollide);
 			/* psb1 nodes vs psb0 faces    */
 			if (this->m_tetras.size() > 0)
 				docollide.useFaceNormal = true;
@@ -4225,8 +4265,8 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
 			docollide.psb[0] = psb;
 			docollide.psb[1] = this;
 			docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
-												 docollide.psb[1]->m_fdbvt.m_root,
-												 docollide);
+												docollide.psb[1]->m_fdbvt.m_root,
+												docollide);
 		}
 		else
 		{
@@ -4236,14 +4276,14 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
 				docollide.mrg = SAFE_EPSILON;
 				docollide.psb[0] = this;
 				docollide.psb[1] = psb;
-                docollide.dt = psb->m_sst.sdt;
+				docollide.dt = psb->m_sst.sdt;
 				if (this->m_tetras.size() > 0)
 					docollide.useFaceNormal = true;
 				else
 					docollide.useFaceNormal = false;
 				/* psb0 faces vs psb0 faces    */
 				calculateNormalCone(this->m_fdbvnt);  // should compute this outside of this scope
-				this->m_fdbvt.selfCollideT(m_fdbvnt,docollide);
+				this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
 			}
 		}
 	}
@@ -4648,44 +4688,43 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
 
 void btSoftBody::updateDeactivation(btScalar timeStep)
 {
-    if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
-        return;
+	if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
+		return;
 
-    if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold)
-    {
-        m_deactivationTime += timeStep;
-    }
-    else
-    {
-        m_deactivationTime = btScalar(0.);
-        setActivationState(0);
-    }
+	if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold)
+	{
+		m_deactivationTime += timeStep;
+	}
+	else
+	{
+		m_deactivationTime = btScalar(0.);
+		setActivationState(0);
+	}
 }
 
-
 void btSoftBody::setZeroVelocity()
 {
-    for (int i = 0; i < m_nodes.size(); ++i)
-    {
-        m_nodes[i].m_v.setZero();
-    }
+	for (int i = 0; i < m_nodes.size(); ++i)
+	{
+		m_nodes[i].m_v.setZero();
+	}
 }
 
 bool btSoftBody::wantsSleeping()
 {
-    if (getActivationState() == DISABLE_DEACTIVATION)
-        return false;
+	if (getActivationState() == DISABLE_DEACTIVATION)
+		return false;
 
-    //disable deactivation
-    if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
-        return false;
+	//disable deactivation
+	if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
+		return false;
 
-    if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
-        return true;
+	if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
+		return true;
 
-    if (m_deactivationTime > gDeactivationTime)
-    {
-        return true;
-    }
-    return false;
+	if (m_deactivationTime > gDeactivationTime)
+	{
+		return true;
+	}
+	return false;
 }