bullet: Sync with upstream 3.24

Remove upstreamed patch.

modules/bullet/SCsub

@@ -152,6 +152,7 @@ if env["builtin_bullet"]:
         "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp",
         "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp",
         "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp",
+        "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp",
         "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp",
         "BulletDynamics/MLCPSolvers/btDantzigLCP.cpp",
         "BulletDynamics/MLCPSolvers/btMLCPSolver.cpp",
@@ -177,6 +178,10 @@ if env["builtin_bullet"]:
         "BulletSoftBody/btDeformableContactProjection.cpp",
         "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp",
         "BulletSoftBody/btDeformableContactConstraint.cpp",
+        "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp",
+        "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp",
+        "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp",
+        "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp",
         "BulletSoftBody/poly34.cpp",
         # clew
         "clew/clew.c",

thirdparty/README.md

@@ -20,7 +20,7 @@ Files extracted from upstream source:
 ## bullet
 
 - Upstream: https://github.com/bulletphysics/bullet3
-- Version: 3.21 (6a59241074720e9df119f2f86bc01765917feb1e, 2021)
+- Version: 3.24 (7dee3436e747958e7088dfdcea0e4ae031ce619e, 2022)
 - License: zlib
 
 Files extracted from upstream source:

thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h

@@ -135,7 +135,11 @@ public:
 
 		int otherSize = otherArray.size();
 		resize(otherSize);
-		otherArray.copy(0, otherSize, m_data);
+		//don't use otherArray.copy, it can leak memory
+		for (int i = 0; i < otherSize; i++)
+		{
+			m_data[i] = otherArray[i];
+		}
 	}
 
 	/// return the number of elements in the array
@@ -506,7 +510,11 @@ public:
 	{
 		int otherSize = otherArray.size();
 		resize(otherSize);
-		otherArray.copy(0, otherSize, m_data);
+		//don't use otherArray.copy, it can leak memory
+		for (int i = 0; i < otherSize; i++)
+		{
+			m_data[i] = otherArray[i];
+		}
 	}
 
 	void removeAtIndex(int index)

thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -198,7 +198,6 @@ void btCollisionWorld::updateAabbs()
 {
 	BT_PROFILE("updateAabbs");
 
-	btTransform predictedTrans;
 	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];

thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -103,6 +103,44 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId,
 
 	if (m_convexBodyWrap->getCollisionShape()->isConvex())
 	{
+#ifndef BT_DISABLE_CONVEX_CONCAVE_EARLY_OUT		
+		//an early out optimisation if the object is separated from the triangle
+		//projected on the triangle normal)
+		{
+			const btVector3 v0 = m_triBodyWrap->getWorldTransform()*triangle[0];
+			const btVector3 v1 = m_triBodyWrap->getWorldTransform()*triangle[1];
+			const btVector3 v2 = m_triBodyWrap->getWorldTransform()*triangle[2];
+
+			btVector3 triangle_normal_world = ( v1 - v0).cross(v2 - v0);
+			triangle_normal_world.normalize();
+
+		    btConvexShape* convex = (btConvexShape*)m_convexBodyWrap->getCollisionShape();
+			
+			btVector3 localPt = convex->localGetSupportingVertex(m_convexBodyWrap->getWorldTransform().getBasis().inverse()*triangle_normal_world);
+			btVector3 worldPt = m_convexBodyWrap->getWorldTransform()*localPt;
+			//now check if this is fully on one side of the triangle
+			btScalar proj_distPt = triangle_normal_world.dot(worldPt);
+			btScalar proj_distTr = triangle_normal_world.dot(v0);
+			btScalar contact_threshold = m_manifoldPtr->getContactBreakingThreshold()+ m_resultOut->m_closestPointDistanceThreshold;
+			btScalar dist = proj_distTr - proj_distPt;
+			if (dist > contact_threshold)
+				return;
+
+			//also check the other side of the triangle
+			triangle_normal_world*=-1;
+
+			localPt = convex->localGetSupportingVertex(m_convexBodyWrap->getWorldTransform().getBasis().inverse()*triangle_normal_world);
+			worldPt = m_convexBodyWrap->getWorldTransform()*localPt;
+			//now check if this is fully on one side of the triangle
+			proj_distPt = triangle_normal_world.dot(worldPt);
+			proj_distTr = triangle_normal_world.dot(v0);
+			
+			dist = proj_distTr - proj_distPt;
+			if (dist > contact_threshold)
+				return;
+        }
+#endif //BT_DISABLE_CONVEX_CONCAVE_EARLY_OUT
+
 		btTriangleShape tm(triangle[0], triangle[1], triangle[2]);
 		tm.setMargin(m_collisionMarginTriangle);
 
@@ -132,7 +170,10 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId,
 			m_resultOut->setShapeIdentifiersB(partId, triangleIndex);
 		}
 
-		colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut);
+		{
+			BT_PROFILE("processCollision (GJK?)");
+			colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut);
+		}
 
 		if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
 		{

thirdparty/bullet/BulletCollision/CollisionShapes/btConvexHullShape.h

@@ -25,6 +25,7 @@ subject to the following restrictions:
 ATTRIBUTE_ALIGNED16(class)
 btConvexHullShape : public btPolyhedralConvexAabbCachingShape
 {
+protected:
 	btAlignedObjectArray<btVector3> m_unscaledPoints;
 
 public:

thirdparty/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.h

@@ -43,6 +43,9 @@ public:
 	void setTransformB(const btTransform& transB) { m_transB = transB; }
 
 	const btTransform& getTransformA() const { return m_transA; }
+	const btTransform& getTransformB() const { return m_transB; }
+
+	// keep this for backward compatibility
 	const btTransform& GetTransformB() const { return m_transB; }
 
 	virtual btScalar getMargin() const;

thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -71,8 +71,8 @@ public:
 					const btVector3& normal,
 					btScalar distance) : m_localPointA(pointA),
 										 m_localPointB(pointB),
-										 m_positionWorldOnB(0,0,0),
+                     m_positionWorldOnB(0,0,0),
-										 m_positionWorldOnA(0,0,0),
+                     m_positionWorldOnA(0,0,0),
 										 m_normalWorldOnB(normal),
 										 m_distance1(distance),
 										 m_combinedFriction(btScalar(0.)),
@@ -95,8 +95,8 @@ public:
 										 m_contactERP(0.f),
 										 m_frictionCFM(0.f),
 										 m_lifeTime(0),
-										 m_lateralFrictionDir1(0,0,0),
+            m_lateralFrictionDir1(0,0,0),
-										 m_lateralFrictionDir2(0,0,0)
+            m_lateralFrictionDir2(0,0,0)
 	{
 	}
 

thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -902,8 +902,8 @@ void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bod
 						btVector3 distVec = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()) * sweepResults.m_closestHitFraction;
 						btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
 
-						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject);
 						btMutexLock(&m_predictiveManifoldsMutex);
+						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject);
 						m_predictiveManifolds.push_back(manifold);
 						btMutexUnlock(&m_predictiveManifoldsMutex);
 

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -31,7 +31,7 @@ enum btTypedMultiBodyConstraintType
 	MULTIBODY_CONSTRAINT_SLIDER,
 	MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
 	MULTIBODY_CONSTRAINT_FIXED,
-	
+	MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT,
 	MAX_MULTIBODY_CONSTRAINT_TYPE,
 };
 

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp

@@ -0,0 +1,270 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+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.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySphericalJointLimit.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+btMultiBodySphericalJointLimit::btMultiBodySphericalJointLimit(btMultiBody* body, int link, 
+	btScalar swingxRange,
+	btScalar swingyRange,
+	btScalar twistRange,
+	btScalar maxAppliedImpulse)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT),
+	m_desiredVelocity(0, 0, 0),
+	m_desiredPosition(0,0,0,1),
+	m_use_multi_dof_params(false),
+	m_kd(1., 1., 1.),
+	m_kp(0.2, 0.2, 0.2),
+	m_erp(1),
+	m_rhsClamp(SIMD_INFINITY),
+	m_maxAppliedImpulseMultiDof(maxAppliedImpulse, maxAppliedImpulse, maxAppliedImpulse),
+	m_pivotA(m_bodyA->getLink(link).m_eVector),
+	m_pivotB(m_bodyB->getLink(link).m_eVector),
+	m_swingxRange(swingxRange),
+	m_swingyRange(swingyRange),
+	m_twistRange(twistRange)
+
+{
+
+	m_maxAppliedImpulse = maxAppliedImpulse;
+}
+
+
+void btMultiBodySphericalJointLimit::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+
+	jacobianB(1)[offset] = -1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+
+btMultiBodySphericalJointLimit::~btMultiBodySphericalJointLimit()
+{
+}
+
+int btMultiBodySphericalJointLimit::getIslandIdA() const
+{
+	if (this->m_linkA < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySphericalJointLimit::getIslandIdB() const
+{
+	if (m_linkB < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodySphericalJointLimit::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												 btMultiBodyJacobianData& data,
+												 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+	
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	const btScalar posError = 0;
+	const btVector3 zero(0, 0, 0);
+
+	
+	btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
+	
+	btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
+		m_bodyA->getJointPosMultiDof(m_linkA)[1],
+		m_bodyA->getJointPosMultiDof(m_linkA)[2],
+		m_bodyA->getJointPosMultiDof(m_linkA)[3]);
+
+	btQuaternion refQuat = m_desiredPosition;
+	btVector3 vTwist(0,0,1);
+	
+	btVector3 vConeNoTwist = quatRotate(currentQuat, vTwist);
+	vConeNoTwist.normalize();
+	btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist);
+	qABCone.normalize();
+	btQuaternion qABTwist = qABCone.inverse() * currentQuat;
+	qABTwist.normalize();
+	btQuaternion desiredQuat = qABTwist;
+
+
+	btQuaternion relRot = currentQuat.inverse() * desiredQuat;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
+	
+	btScalar limitRanges[3] = {m_swingxRange, m_swingyRange, m_twistRange};
+	
+	/// twist axis/angle
+	btQuaternion qMinTwist = qABTwist;
+	btScalar twistAngle = qABTwist.getAngle();
+
+	if (twistAngle > SIMD_PI)  // long way around. flip quat and recalculate.
+	{
+		qMinTwist = -(qABTwist);
+		twistAngle = qMinTwist.getAngle();
+	}
+	btVector3 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+	if (twistAngle > SIMD_EPSILON)
+		vTwistAxis.normalize();
+	
+	if (vTwistAxis.dot(vTwist)<0)
+		twistAngle*=-1.;
+
+	angleDiff[2] = twistAngle;
+
+
+	for (int row = 0; row < getNumRows(); row++)
+	{
+		btScalar allowed = limitRanges[row];
+		btScalar damp = 1;
+		if((angleDiff[row]>-allowed)&&(angleDiff[row]<allowed))
+		{
+			angleDiff[row]=0;
+			damp=0;
+
+		} else
+		{
+			if (angleDiff[row]>allowed)
+			{
+				angleDiff[row]-=allowed;
+				
+			}
+			if (angleDiff[row]<-allowed)
+			{
+				angleDiff[row]+=allowed;
+				
+			} 
+		}
+		
+
+		int dof = row;
+		
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar desiredVelocity = this->m_desiredVelocity[row];
+		
+		double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
+		btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
+
+		btMatrix3x3 frameAworld;
+		frameAworld.setIdentity();
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+		btScalar posError = 0;
+		{
+			btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eSpherical:
+				{
+					btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
+					double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
+					posError = kp*angleDiff[row % 3];
+					double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse;
+					//should multiply by time step
+					//max_applied_impulse *= infoGlobal.m_timeStep
+
+					double min_applied_impulse = -max_applied_impulse;
+					
+
+					if (posError>0)
+						max_applied_impulse=0;
+					else
+						min_applied_impulse=0;
+
+					if (btFabs(posError)>SIMD_EPSILON)
+					{
+						btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+						fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+							zero, zero, zero,//pure angular, so zero out linear parts
+							posError,
+							infoGlobal,
+							min_applied_impulse, max_applied_impulse, true,
+							1.0, false, 0, 0,
+							damp);
+						constraintRow.m_orgConstraint = this;
+						constraintRow.m_orgDofIndex = row;
+					}
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}
+
+
+void btMultiBodySphericalJointLimit::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+}

thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h

@@ -0,0 +1,115 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+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.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H
+#define BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodySphericalJointLimit : public btMultiBodyConstraint
+{
+protected:
+	btVector3 m_desiredVelocity;
+	btQuaternion m_desiredPosition;
+	bool m_use_multi_dof_params;
+	btVector3 m_kd;
+	btVector3 m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
+	btVector3 m_maxAppliedImpulseMultiDof;
+	btVector3 m_pivotA;
+	btVector3 m_pivotB;
+	btScalar m_swingxRange;
+	btScalar m_swingyRange;
+	btScalar m_twistRange;
+
+public:
+	btMultiBodySphericalJointLimit(btMultiBody* body, int link, 
+		btScalar swingxRange,
+		btScalar swingyRange,
+		btScalar twistRange,
+		btScalar maxAppliedImpulse);
+	
+	virtual ~btMultiBodySphericalJointLimit();
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.0)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = btVector3(kd, kd, kd);
+		m_use_multi_dof_params = false;
+	}
+
+	virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0))
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+		m_use_multi_dof_params = true;
+	}
+
+	virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = btVector3(kp, kp, kp);
+		m_use_multi_dof_params = false;
+	}
+
+	virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f))
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+		m_use_multi_dof_params = true;
+	}
+
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
+
+	btScalar getMaxAppliedImpulseMultiDof(int i) const
+	{
+		return m_maxAppliedImpulseMultiDof[i];
+	}
+
+	void setMaxAppliedImpulseMultiDof(const btVector3& maxImp)
+	{
+		m_maxAppliedImpulseMultiDof = maxImp;
+		m_use_multi_dof_params = true;
+	}
+
+
+	virtual void debugDraw(class btIDebugDraw* drawer);
+
+};
+
+#endif  //BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H

thirdparty/bullet/BulletInverseDynamics/IDMath.cpp

@@ -503,8 +503,8 @@ vec3 rpyFromMatrix(const mat33 &rot)
 {
 	vec3 rpy;
 	rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0));
-	rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0));
 	rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2));
+	rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0));
 	return rpy;
 }
 }  // namespace btInverseDynamics

thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp

@@ -0,0 +1,792 @@
+#include "btReducedDeformableBody.h"
+#include "../btSoftBodyInternals.h"
+#include "btReducedDeformableBodyHelpers.h"
+#include "LinearMath/btTransformUtil.h"
+#include <iostream>
+#include <fstream>
+
+btReducedDeformableBody::btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
+ : btSoftBody(worldInfo, node_count, x, m), m_rigidOnly(false)
+{
+  // reduced deformable
+  m_reducedModel = true;
+  m_nReduced = 0;
+  m_nFull = 0;
+  m_nodeIndexOffset = 0;
+
+  m_transform_lock = false;
+  m_ksScale = 1.0;
+  m_rhoScale = 1.0;
+
+  // rigid motion
+  m_linearVelocity.setZero();
+	m_angularVelocity.setZero();
+  m_internalDeltaLinearVelocity.setZero();
+  m_internalDeltaAngularVelocity.setZero();
+  m_angularVelocityFromReduced.setZero();
+  m_internalDeltaAngularVelocityFromReduced.setZero();
+	m_angularFactor.setValue(1, 1, 1);
+	m_linearFactor.setValue(1, 1, 1);
+  // m_invInertiaLocal.setValue(1, 1, 1);
+  m_invInertiaLocal.setIdentity();
+  m_mass = 0.0;
+  m_inverseMass = 0.0;
+
+  m_linearDamping = 0;
+  m_angularDamping = 0;
+
+  // Rayleigh damping
+  m_dampingAlpha = 0;
+  m_dampingBeta = 0;
+
+  m_rigidTransformWorld.setIdentity();
+}
+
+void btReducedDeformableBody::setReducedModes(int num_modes, int full_size)
+{
+  m_nReduced = num_modes;
+  m_nFull = full_size;
+  m_reducedDofs.resize(m_nReduced, 0);
+  m_reducedDofsBuffer.resize(m_nReduced, 0);
+  m_reducedVelocity.resize(m_nReduced, 0);
+  m_reducedVelocityBuffer.resize(m_nReduced, 0);
+  m_reducedForceElastic.resize(m_nReduced, 0);
+  m_reducedForceDamping.resize(m_nReduced, 0);
+  m_reducedForceExternal.resize(m_nReduced, 0);
+  m_internalDeltaReducedVelocity.resize(m_nReduced, 0);
+  m_nodalMass.resize(full_size, 0);
+  m_localMomentArm.resize(m_nFull);
+}
+
+void btReducedDeformableBody::setMassProps(const tDenseArray& mass_array)
+{
+  btScalar total_mass = 0;
+  btVector3 CoM(0, 0, 0);
+	for (int i = 0; i < m_nFull; ++i)
+	{
+		m_nodalMass[i] = m_rhoScale * mass_array[i];
+		m_nodes[i].m_im = mass_array[i] > 0 ? 1.0 / (m_rhoScale * mass_array[i]) : 0;
+		total_mass += m_rhoScale * mass_array[i];
+
+    CoM += m_nodalMass[i] * m_nodes[i].m_x;
+	}
+  // total rigid body mass
+  m_mass = total_mass;
+  m_inverseMass = total_mass > 0 ? 1.0 / total_mass : 0;
+  // original CoM
+  m_initialCoM = CoM / total_mass;
+}
+
+void btReducedDeformableBody::setInertiaProps()
+{
+  // make sure the initial CoM is at the origin (0,0,0)
+  // for (int i = 0; i < m_nFull; ++i)
+  // {
+  //   m_nodes[i].m_x -= m_initialCoM;
+  // }
+  // m_initialCoM.setZero();
+  m_rigidTransformWorld.setOrigin(m_initialCoM);
+  m_interpolationWorldTransform = m_rigidTransformWorld;
+  
+  updateLocalInertiaTensorFromNodes();
+
+  // update world inertia tensor
+  btMatrix3x3 rotation;
+  rotation.setIdentity();
+  updateInitialInertiaTensor(rotation);
+  updateInertiaTensor();
+  m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
+}
+
+void btReducedDeformableBody::setRigidVelocity(const btVector3& v)
+{
+  m_linearVelocity = v;
+}
+
+void btReducedDeformableBody::setRigidAngularVelocity(const btVector3& omega)
+{
+  m_angularVelocity = omega;
+}
+
+void btReducedDeformableBody::setStiffnessScale(const btScalar ks)
+{
+  m_ksScale = ks;
+}
+
+void btReducedDeformableBody::setMassScale(const btScalar rho)
+{
+  m_rhoScale = rho;
+}
+
+void btReducedDeformableBody::setFixedNodes(const int n_node)
+{
+  m_fixedNodes.push_back(n_node);
+  m_nodes[n_node].m_im = 0;   // set inverse mass to be zero for the constraint solver.
+}
+
+void btReducedDeformableBody::setDamping(const btScalar alpha, const btScalar beta)
+{
+  m_dampingAlpha = alpha;
+  m_dampingBeta = beta;
+}
+
+void btReducedDeformableBody::internalInitialization()
+{
+  // zeroing
+  endOfTimeStepZeroing();
+  // initialize rest position
+  updateRestNodalPositions();
+  // initialize local nodal moment arm form the CoM
+  updateLocalMomentArm();
+  // initialize projection matrix
+  updateExternalForceProjectMatrix(false);
+}
+
+void btReducedDeformableBody::updateLocalMomentArm()
+{
+  TVStack delta_x;
+  delta_x.resize(m_nFull);
+
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    for (int k = 0; k < 3; ++k)
+    {
+      // compute displacement
+      delta_x[i][k] = 0;
+      for (int j = 0; j < m_nReduced; ++j) 
+      {
+        delta_x[i][k] += m_modes[j][3 * i + k] * m_reducedDofs[j];
+      }
+    }
+    // get new moment arm Sq + x0
+    m_localMomentArm[i] = m_x0[i] - m_initialCoM + delta_x[i];
+  }
+}
+
+void btReducedDeformableBody::updateExternalForceProjectMatrix(bool initialized)
+{
+  // if not initialized, need to compute both P_A and Cq
+  // otherwise, only need to udpate Cq
+  if (!initialized)
+  {
+    // resize
+    m_projPA.resize(m_nReduced);
+    m_projCq.resize(m_nReduced);
+
+    m_STP.resize(m_nReduced);
+    m_MrInvSTP.resize(m_nReduced);
+
+    // P_A
+    for (int r = 0; r < m_nReduced; ++r)
+    {
+      m_projPA[r].resize(3 * m_nFull, 0);
+      for (int i = 0; i < m_nFull; ++i)
+      {
+        btMatrix3x3 mass_scaled_i = Diagonal(1) - Diagonal(m_nodalMass[i] / m_mass);
+        btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
+        btVector3 prod_i = mass_scaled_i * s_ri;
+
+        for (int k = 0; k < 3; ++k)
+          m_projPA[r][3 * i + k] = prod_i[k];
+
+        // btScalar ratio = m_nodalMass[i] / m_mass;
+        // m_projPA[r] += btVector3(- m_modes[r][3 * i] * ratio,
+        //                          - m_modes[r][3 * i + 1] * ratio,
+        //                          - m_modes[r][3 * i + 2] * ratio);
+      }
+    }
+  }
+
+  // C(q) is updated once per position update
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+  	m_projCq[r].resize(3 * m_nFull, 0);
+    for (int i = 0; i < m_nFull; ++i)
+    {
+      btMatrix3x3 r_star = Cross(m_localMomentArm[i]);
+      btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
+      btVector3 prod_i = r_star * m_invInertiaTensorWorld * r_star * s_ri;
+
+      for (int k = 0; k < 3; ++k)
+        m_projCq[r][3 * i + k] = m_nodalMass[i] * prod_i[k];
+
+      // btVector3 si(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
+      // m_projCq[r] += m_nodalMass[i] * si.cross(m_localMomentArm[i]);
+    }
+  }
+}
+
+void btReducedDeformableBody::endOfTimeStepZeroing()
+{
+  for (int i = 0; i < m_nReduced; ++i)
+  {
+    m_reducedForceElastic[i] = 0;
+    m_reducedForceDamping[i] = 0;
+    m_reducedForceExternal[i] = 0;
+    m_internalDeltaReducedVelocity[i] = 0;
+    m_reducedDofsBuffer[i] = m_reducedDofs[i];
+    m_reducedVelocityBuffer[i] = m_reducedVelocity[i];
+  }
+  // std::cout << "zeroed!\n";
+}
+
+void btReducedDeformableBody::applyInternalVelocityChanges()
+{
+  m_linearVelocity += m_internalDeltaLinearVelocity;
+  m_angularVelocity += m_internalDeltaAngularVelocity;
+  m_internalDeltaLinearVelocity.setZero();
+  m_internalDeltaAngularVelocity.setZero();
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    m_reducedVelocity[r] += m_internalDeltaReducedVelocity[r];
+    m_internalDeltaReducedVelocity[r] = 0;
+  }
+}
+
+void btReducedDeformableBody::predictIntegratedTransform(btScalar dt, btTransform& predictedTransform)
+{
+	btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, predictedTransform);
+}
+
+void btReducedDeformableBody::updateReducedDofs(btScalar solverdt)
+{
+  for (int r = 0; r < m_nReduced; ++r)
+  { 
+    m_reducedDofs[r] = m_reducedDofsBuffer[r] + solverdt * m_reducedVelocity[r];
+  }
+}
+
+void btReducedDeformableBody::mapToFullPosition(const btTransform& ref_trans)
+{
+  btVector3 origin = ref_trans.getOrigin();
+  btMatrix3x3 rotation = ref_trans.getBasis();
+  
+
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    m_nodes[i].m_x = rotation * m_localMomentArm[i] + origin;
+    m_nodes[i].m_q = m_nodes[i].m_x;
+  }
+}
+
+void btReducedDeformableBody::updateReducedVelocity(btScalar solverdt)
+{
+  // update reduced velocity
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    // the reduced mass is always identity!
+    btScalar delta_v = 0;
+    delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r]);
+    // delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r] + m_reducedForceExternal[r]);
+    m_reducedVelocity[r] = m_reducedVelocityBuffer[r] + delta_v;
+  }
+}
+
+void btReducedDeformableBody::mapToFullVelocity(const btTransform& ref_trans)
+{
+  // compute the reduced contribution to the angular velocity
+  // btVector3 sum_linear(0, 0, 0);
+  // btVector3 sum_angular(0, 0, 0);
+  // m_linearVelocityFromReduced.setZero();
+  // m_angularVelocityFromReduced.setZero();
+  // for (int i = 0; i < m_nFull; ++i)
+  // {
+  //   btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i];
+  //   btMatrix3x3 r_star = Cross(r_com);
+
+  //   btVector3 v_from_reduced(0, 0, 0);
+  //   for (int k = 0; k < 3; ++k)
+  //   {
+  //     for (int r = 0; r < m_nReduced; ++r)
+  //     {
+  //       v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
+  //     }
+  //   }
+
+  //   btVector3 delta_linear = m_nodalMass[i] * v_from_reduced;
+  //   btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced);
+  //   sum_linear += delta_linear;
+  //   sum_angular += delta_angular;
+  //   // std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n";
+  //   // std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n";
+  //   // std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n";
+  //   // std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n";
+  // }
+  // m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear);
+  // m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular;
+
+  // m_linearVelocity -= m_linearVelocityFromReduced;
+  // m_angularVelocity -= m_angularVelocityFromReduced;
+
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i);
+  }
+}
+
+const btVector3 btReducedDeformableBody::computeTotalAngularMomentum() const
+{
+  btVector3 L_rigid = m_invInertiaTensorWorld.inverse() * m_angularVelocity;
+  btVector3 L_reduced(0, 0, 0);
+  btMatrix3x3 omega_prime_star = Cross(m_angularVelocityFromReduced);
+
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    btVector3 r_com = m_rigidTransformWorld.getBasis() * m_localMomentArm[i];
+    btMatrix3x3 r_star = Cross(r_com);
+
+    btVector3 v_from_reduced(0, 0, 0);
+    for (int k = 0; k < 3; ++k)
+    {
+      for (int r = 0; r < m_nReduced; ++r)
+      {
+        v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
+      }
+    }
+
+    L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced - omega_prime_star * r_com));
+    // L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced));
+  }
+  return L_rigid + L_reduced;
+}
+
+const btVector3 btReducedDeformableBody::computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const
+{
+  btVector3 v_from_reduced(0, 0, 0);
+  btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node];
+  // compute velocity contributed by the reduced velocity
+  for (int k = 0; k < 3; ++k)
+  {
+    for (int r = 0; r < m_nReduced; ++r)
+    {
+      v_from_reduced[k] += m_modes[r][3 * n_node + k] * m_reducedVelocity[r];
+    }
+  }
+  // get new velocity
+  btVector3 vel = m_angularVelocity.cross(r_com) + 
+                  ref_trans.getBasis() * v_from_reduced +
+                  m_linearVelocity;
+  return vel;
+}
+
+const btVector3 btReducedDeformableBody::internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const
+{
+  btVector3 deltaV_from_reduced(0, 0, 0);
+  btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node];
+
+  // compute velocity contributed by the reduced velocity
+  for (int k = 0; k < 3; ++k)
+  {
+    for (int r = 0; r < m_nReduced; ++r)
+    {
+      deltaV_from_reduced[k] += m_modes[r][3 * n_node + k] * m_internalDeltaReducedVelocity[r];
+    }
+  }
+
+  // get delta velocity
+  btVector3 deltaV = m_internalDeltaAngularVelocity.cross(r_com) + 
+                     ref_trans.getBasis() * deltaV_from_reduced +
+                     m_internalDeltaLinearVelocity;
+  return deltaV;
+}
+
+void btReducedDeformableBody::proceedToTransform(btScalar dt, bool end_of_time_step)
+{
+  btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, m_interpolationWorldTransform);
+  updateInertiaTensor();
+  // m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose();
+  m_rigidTransformWorld = m_interpolationWorldTransform;
+  m_invInertiaTensorWorld = m_interpolateInvInertiaTensorWorld;
+}
+
+void btReducedDeformableBody::transformTo(const btTransform& trs)
+{
+	btTransform current_transform = getRigidTransform();
+	btTransform new_transform(trs.getBasis() * current_transform.getBasis().transpose(),
+                            trs.getOrigin() - current_transform.getOrigin());
+  transform(new_transform);
+}
+
+void btReducedDeformableBody::transform(const btTransform& trs)
+{
+  m_transform_lock = true;
+
+  // transform mesh
+  {
+    const btScalar margin = getCollisionShape()->getMargin();
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+
+    btVector3 CoM = m_rigidTransformWorld.getOrigin();
+    btVector3 translation = trs.getOrigin();
+    btMatrix3x3 rotation = trs.getBasis();
+
+    for (int i = 0; i < m_nodes.size(); ++i)
+    {
+      Node& n = m_nodes[i];
+      n.m_x = rotation * (n.m_x - CoM) + CoM + translation;
+      n.m_q = rotation * (n.m_q - CoM) + CoM + translation;
+      n.m_n = rotation * n.m_n;
+      vol = btDbvtVolume::FromCR(n.m_x, margin);
+
+      m_ndbvt.update(n.m_leaf, vol);
+    }
+    updateNormals();
+    updateBounds();
+    updateConstants();
+  }
+
+  // update modes
+  updateModesByRotation(trs.getBasis());
+
+  // update inertia tensor
+  updateInitialInertiaTensor(trs.getBasis());
+  updateInertiaTensor();
+  m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
+  
+  // update rigid frame (No need to update the rotation. Nodes have already been updated.)
+  m_rigidTransformWorld.setOrigin(m_initialCoM + trs.getOrigin());
+  m_interpolationWorldTransform = m_rigidTransformWorld;
+  m_initialCoM = m_rigidTransformWorld.getOrigin();
+
+  internalInitialization();
+}
+
+void btReducedDeformableBody::scale(const btVector3& scl)
+{
+  // Scaling the mesh after transform is applied is not allowed
+  btAssert(!m_transform_lock);
+
+  // scale the mesh
+  {
+    const btScalar margin = getCollisionShape()->getMargin();
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+
+    btVector3 CoM = m_rigidTransformWorld.getOrigin();
+
+    for (int i = 0; i < m_nodes.size(); ++i)
+    {
+      Node& n = m_nodes[i];
+      n.m_x = (n.m_x - CoM) * scl + CoM;
+      n.m_q = (n.m_q - CoM) * scl + CoM;
+      vol = btDbvtVolume::FromCR(n.m_x, margin);
+      m_ndbvt.update(n.m_leaf, vol);
+    }
+    updateNormals();
+    updateBounds();
+    updateConstants();
+    initializeDmInverse();
+  }
+
+  // update inertia tensor
+  updateLocalInertiaTensorFromNodes();
+
+  btMatrix3x3 id;
+  id.setIdentity();
+  updateInitialInertiaTensor(id);   // there is no rotation, but the local inertia tensor has changed
+  updateInertiaTensor();
+  m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
+
+  internalInitialization();
+}
+
+void btReducedDeformableBody::setTotalMass(btScalar mass, bool fromfaces)
+{
+  // Changing the total mass after transform is applied is not allowed
+  btAssert(!m_transform_lock);
+
+  btScalar scale_ratio = mass / m_mass;
+
+  // update nodal mass
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    m_nodalMass[i] *= scale_ratio;
+  }
+  m_mass = mass;
+  m_inverseMass = mass > 0 ? 1.0 / mass : 0;
+
+  // update inertia tensors
+  updateLocalInertiaTensorFromNodes();
+
+  btMatrix3x3 id;
+  id.setIdentity();
+  updateInitialInertiaTensor(id);   // there is no rotation, but the local inertia tensor has changed
+  updateInertiaTensor();
+  m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
+
+  internalInitialization();
+}
+
+void btReducedDeformableBody::updateRestNodalPositions()
+{
+  // update reset nodal position
+  m_x0.resize(m_nFull);
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    m_x0[i] = m_nodes[i].m_x;
+  }
+}
+
+// reference notes:
+// https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-07-dynamics-fall-2009/lecture-notes/MIT16_07F09_Lec26.pdf
+void btReducedDeformableBody::updateLocalInertiaTensorFromNodes()
+{
+  btMatrix3x3 inertia_tensor;
+  inertia_tensor.setZero();
+
+  for (int p = 0; p < m_nFull; ++p)
+  {
+    btMatrix3x3 particle_inertia;
+    particle_inertia.setZero();
+
+    btVector3 r = m_nodes[p].m_x - m_initialCoM;
+
+    particle_inertia[0][0] = m_nodalMass[p] * (r[1] * r[1] + r[2] * r[2]);
+    particle_inertia[1][1] = m_nodalMass[p] * (r[0] * r[0] + r[2] * r[2]);
+    particle_inertia[2][2] = m_nodalMass[p] * (r[0] * r[0] + r[1] * r[1]);
+
+    particle_inertia[0][1] = - m_nodalMass[p] * (r[0] * r[1]);
+    particle_inertia[0][2] = - m_nodalMass[p] * (r[0] * r[2]);
+    particle_inertia[1][2] = - m_nodalMass[p] * (r[1] * r[2]);
+
+    particle_inertia[1][0] = particle_inertia[0][1];
+    particle_inertia[2][0] = particle_inertia[0][2];
+    particle_inertia[2][1] = particle_inertia[1][2];
+
+    inertia_tensor += particle_inertia;
+  }
+  m_invInertiaLocal = inertia_tensor.inverse();
+}
+
+void btReducedDeformableBody::updateInitialInertiaTensor(const btMatrix3x3& rotation)
+{
+  // m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose();
+  m_invInertiaTensorWorldInitial = rotation * m_invInertiaLocal * rotation.transpose();
+}
+
+void btReducedDeformableBody::updateModesByRotation(const btMatrix3x3& rotation)
+{
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    for (int i = 0; i < m_nFull; ++i)
+    {
+      btVector3 nodal_disp(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
+      nodal_disp = rotation * nodal_disp;
+
+      for (int k = 0; k < 3; ++k)
+      {
+        m_modes[r][3 * i + k] = nodal_disp[k];
+      }
+    }
+  }
+}
+
+void btReducedDeformableBody::updateInertiaTensor()
+{
+	m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis() * m_invInertiaTensorWorldInitial * m_rigidTransformWorld.getBasis().transpose();
+}
+
+void btReducedDeformableBody::applyDamping(btScalar timeStep)
+{
+  m_linearVelocity *= btScalar(1) - m_linearDamping;
+  m_angularDamping *= btScalar(1) - m_angularDamping;
+}
+
+void btReducedDeformableBody::applyCentralImpulse(const btVector3& impulse)
+{
+  m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
+  #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+  clampVelocity(m_linearVelocity);
+  #endif
+}
+
+void btReducedDeformableBody::applyTorqueImpulse(const btVector3& torque)
+{
+  m_angularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor;
+  #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+  clampVelocity(m_angularVelocity);
+  #endif
+}
+
+void btReducedDeformableBody::internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos)
+{
+  if (m_inverseMass == btScalar(0.))
+  {
+    std::cout << "something went wrong...probably didn't initialize?\n";
+    btAssert(false);
+  }
+  // delta linear velocity
+  m_internalDeltaLinearVelocity += impulse * m_linearFactor * m_inverseMass;
+  // delta angular velocity
+  btVector3 torque = rel_pos.cross(impulse * m_linearFactor);
+  m_internalDeltaAngularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor;
+}
+
+btVector3 btReducedDeformableBody::getRelativePos(int n_node)
+{
+  btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis();
+  btVector3 ri = rotation * m_localMomentArm[n_node];
+  return ri;
+}
+
+btMatrix3x3 btReducedDeformableBody::getImpulseFactor(int n_node)
+{
+  // relative position
+  btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis();
+  btVector3 ri = rotation * m_localMomentArm[n_node];
+  btMatrix3x3 ri_skew = Cross(ri);
+
+  // calculate impulse factor
+  // rigid part
+  btScalar inv_mass = m_nodalMass[n_node] > btScalar(0) ? btScalar(1) / m_mass : btScalar(0);
+  btMatrix3x3 K1 = Diagonal(inv_mass);
+  K1 -= ri_skew * m_interpolateInvInertiaTensorWorld * ri_skew;
+
+  // reduced deformable part
+  btMatrix3x3 SA;
+  SA.setZero();
+  for (int i = 0; i < 3; ++i)
+  {
+    for (int j = 0; j < 3; ++j)
+    {
+      for (int r = 0; r < m_nReduced; ++r)
+      {
+        SA[i][j] += m_modes[r][3 * n_node + i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]);
+      }
+    }
+  }
+  btMatrix3x3 RSARinv = rotation * SA * rotation.transpose();
+
+
+  TVStack omega_helper; // Sum_i m_i r*_i R S_i
+  omega_helper.resize(m_nReduced);
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    omega_helper[r].setZero();
+    for (int i = 0; i < m_nFull; ++i)
+    {
+      btMatrix3x3 mi_rstar_i = rotation * Cross(m_localMomentArm[i]) * m_nodalMass[i];
+      btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
+      omega_helper[r] += mi_rstar_i * rotation * s_ri;
+    }
+  }
+
+  btMatrix3x3 sum_multiply_A;
+  sum_multiply_A.setZero();
+  for (int i = 0; i < 3; ++i)
+  {
+    for (int j = 0; j < 3; ++j)
+    {
+      for (int r = 0; r < m_nReduced; ++r)
+      {
+        sum_multiply_A[i][j] += omega_helper[r][i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]);
+      }
+    }
+  }
+
+  btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose();
+
+  return m_rigidOnly ? K1 : K1 + K2;
+}
+
+void btReducedDeformableBody::internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt)
+{
+  if (!m_rigidOnly)
+  {
+    // apply impulse force
+    applyFullSpaceNodalForce(impulse / dt, n_node);
+
+    // update delta damping force
+    tDenseArray reduced_vel_tmp;
+    reduced_vel_tmp.resize(m_nReduced);
+    for (int r = 0; r < m_nReduced; ++r)
+    {
+      reduced_vel_tmp[r] = m_reducedVelocity[r] + m_internalDeltaReducedVelocity[r];
+    }
+    applyReducedDampingForce(reduced_vel_tmp);
+    // applyReducedDampingForce(m_internalDeltaReducedVelocity);
+
+    // delta reduced velocity
+    for (int r = 0; r < m_nReduced; ++r)
+    {
+      // The reduced mass is always identity!
+      m_internalDeltaReducedVelocity[r] += dt * (m_reducedForceDamping[r] + m_reducedForceExternal[r]);
+    }
+  }
+
+  internalApplyRigidImpulse(impulse, rel_pos);
+}
+
+void btReducedDeformableBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_node)
+{
+  // f_local = R^-1 * f_ext //TODO: interpoalted transfrom
+  // btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext;
+  btVector3 f_local = m_interpolationWorldTransform.getBasis().transpose() * f_ext;
+
+  // f_ext_r = [S^T * P]_{n_node} * f_local
+  tDenseArray f_ext_r;
+  f_ext_r.resize(m_nReduced, 0);
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    m_reducedForceExternal[r] = 0;
+    for (int k = 0; k < 3; ++k)
+    {
+      f_ext_r[r] += (m_projPA[r][3 * n_node + k] + m_projCq[r][3 * n_node + k]) * f_local[k];
+    }
+
+    m_reducedForceExternal[r] += f_ext_r[r];
+  }
+}
+
+void btReducedDeformableBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
+{
+  // update rigid frame velocity
+  m_linearVelocity += dt * gravity;
+}
+
+void btReducedDeformableBody::applyReducedElasticForce(const tDenseArray& reduce_dofs)
+{
+  for (int r = 0; r < m_nReduced; ++r) 
+  {
+    m_reducedForceElastic[r] = - m_ksScale * m_Kr[r] * reduce_dofs[r];
+  }
+}
+
+void btReducedDeformableBody::applyReducedDampingForce(const tDenseArray& reduce_vel)
+{
+  for (int r = 0; r < m_nReduced; ++r) 
+  {
+    m_reducedForceDamping[r] = - m_dampingBeta * m_ksScale * m_Kr[r] * reduce_vel[r];
+  }
+}
+
+btScalar btReducedDeformableBody::getTotalMass() const
+{
+  return m_mass;
+}
+
+btTransform& btReducedDeformableBody::getRigidTransform()
+{
+  return m_rigidTransformWorld;
+}
+
+const btVector3& btReducedDeformableBody::getLinearVelocity() const
+{
+  return m_linearVelocity;
+}
+
+const btVector3& btReducedDeformableBody::getAngularVelocity() const
+{
+  return m_angularVelocity;
+}
+
+void btReducedDeformableBody::disableReducedModes(const bool rigid_only)
+{
+  m_rigidOnly = rigid_only;
+}
+
+bool btReducedDeformableBody::isReducedModesOFF() const
+{
+  return m_rigidOnly;
+}

thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h

@@ -0,0 +1,257 @@
+#ifndef BT_REDUCED_SOFT_BODY_H
+#define BT_REDUCED_SOFT_BODY_H
+
+#include "../btSoftBody.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btTransform.h"
+
+// Reduced deformable body is a simplified deformable object embedded in a rigid frame.
+class btReducedDeformableBody : public btSoftBody
+{
+ public:
+  //
+  //  Typedefs
+  //
+  typedef btAlignedObjectArray<btVector3> TVStack;
+  // typedef btAlignedObjectArray<btMatrix3x3> tBlockDiagMatrix;
+  typedef btAlignedObjectArray<btScalar> tDenseArray;
+  typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
+
+ private:
+  // flag to turn off the reduced modes
+  bool m_rigidOnly;
+
+  // Flags for transform. Once transform is applied, users cannot scale the mesh or change its total mass.
+  bool m_transform_lock;
+
+  // scaling factors
+  btScalar m_rhoScale;         // mass density scale
+  btScalar m_ksScale;          // stiffness scale
+
+  // projection matrix
+  tDenseMatrix m_projPA;        // Eqn. 4.11 from Rahul Sheth's thesis
+  tDenseMatrix m_projCq;
+  tDenseArray m_STP;
+  tDenseArray m_MrInvSTP;
+
+  TVStack m_localMomentArm; // Sq + x0
+
+  btVector3 m_internalDeltaLinearVelocity;
+  btVector3 m_internalDeltaAngularVelocity;
+  tDenseArray m_internalDeltaReducedVelocity;
+  
+  btVector3 m_linearVelocityFromReduced;  // contribution to the linear velocity from reduced velocity
+  btVector3 m_angularVelocityFromReduced; // contribution to the angular velocity from reduced velocity
+  btVector3 m_internalDeltaAngularVelocityFromReduced;
+
+ protected:
+  // rigid frame
+  btScalar m_mass;          // total mass of the rigid frame
+  btScalar m_inverseMass;   // inverse of the total mass of the rigid frame
+  btVector3 m_linearVelocity;
+  btVector3 m_angularVelocity;
+  btScalar m_linearDamping;    // linear damping coefficient
+  btScalar m_angularDamping;    // angular damping coefficient
+  btVector3 m_linearFactor;
+  btVector3 m_angularFactor;
+  // btVector3 m_invInertiaLocal;
+  btMatrix3x3 m_invInertiaLocal;
+  btTransform m_rigidTransformWorld;
+  btMatrix3x3 m_invInertiaTensorWorldInitial;
+  btMatrix3x3 m_invInertiaTensorWorld;
+  btMatrix3x3 m_interpolateInvInertiaTensorWorld;
+  btVector3 m_initialCoM;  // initial center of mass (original of the m_rigidTransformWorld)
+
+  // damping
+  btScalar m_dampingAlpha;
+  btScalar m_dampingBeta;
+
+ public:
+  //
+  //  Fields
+  //
+
+  // reduced space
+  int m_nReduced;
+  int m_nFull;
+  tDenseMatrix m_modes;														// modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes
+  tDenseArray m_reducedDofs;				   // Reduced degree of freedom
+  tDenseArray m_reducedDofsBuffer;     // Reduced degree of freedom at t^n
+  tDenseArray m_reducedVelocity;		   // Reduced velocity array
+  tDenseArray m_reducedVelocityBuffer; // Reduced velocity array at t^n
+  tDenseArray m_reducedForceExternal;          // reduced external force
+  tDenseArray m_reducedForceElastic;           // reduced internal elastic force
+  tDenseArray m_reducedForceDamping;           // reduced internal damping force
+  tDenseArray m_eigenvalues;		// eigenvalues of the reduce deformable model
+  tDenseArray m_Kr;	// reduced stiffness matrix
+  
+  // full space
+  TVStack m_x0;					     				 // Rest position
+  tDenseArray m_nodalMass;           // Mass on each node
+  btAlignedObjectArray<int> m_fixedNodes; // index of the fixed nodes
+  int m_nodeIndexOffset;             // offset of the node index needed for contact solver when there are multiple reduced deformable body in the world.
+
+  // contacts
+  btAlignedObjectArray<int> m_contactNodesList;
+
+  //
+  // Api
+  //
+  btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m);
+
+  ~btReducedDeformableBody() {}
+
+  //
+  // initializing helpers
+  //
+  void internalInitialization();
+
+  void setReducedModes(int num_modes, int full_size);
+
+  void setMassProps(const tDenseArray& mass_array);
+
+  void setInertiaProps();
+
+  void setRigidVelocity(const btVector3& v);
+
+  void setRigidAngularVelocity(const btVector3& omega);
+
+  void setStiffnessScale(const btScalar ks);
+
+  void setMassScale(const btScalar rho);
+
+  void setFixedNodes(const int n_node);
+
+  void setDamping(const btScalar alpha, const btScalar beta);
+
+  void disableReducedModes(const bool rigid_only);
+
+  virtual void setTotalMass(btScalar mass, bool fromfaces = false);
+
+  //
+  // various internal updates
+  //
+  virtual void transformTo(const btTransform& trs);
+  virtual void transform(const btTransform& trs);
+  // caution: 
+  // need to use scale before using transform, because the scale is performed in the local frame 
+  // (i.e., may have some rotation already, but the m_rigidTransformWorld doesn't have this info)
+  virtual void scale(const btVector3& scl);
+
+ private:
+  void updateRestNodalPositions();
+
+  void updateInitialInertiaTensor(const btMatrix3x3& rotation);
+
+  void updateLocalInertiaTensorFromNodes();
+
+  void updateInertiaTensor();
+
+  void updateModesByRotation(const btMatrix3x3& rotation);
+ 
+ public:
+  void updateLocalMomentArm();
+
+  void predictIntegratedTransform(btScalar dt, btTransform& predictedTransform);
+
+  // update the external force projection matrix 
+  void updateExternalForceProjectMatrix(bool initialized);
+
+  void endOfTimeStepZeroing();
+
+  void applyInternalVelocityChanges();
+
+  //
+  // position and velocity update related
+  //
+
+  // compute reduced degree of freedoms
+  void updateReducedDofs(btScalar solverdt);
+
+  // compute reduced velocity update (for explicit time stepping)
+  void updateReducedVelocity(btScalar solverdt);
+
+  // map to full degree of freedoms
+  void mapToFullPosition(const btTransform& ref_trans);
+
+  // compute full space velocity from the reduced velocity
+  void mapToFullVelocity(const btTransform& ref_trans);
+
+  // compute total angular momentum
+  const btVector3 computeTotalAngularMomentum() const;
+
+  // get a single node's full space velocity from the reduced velocity
+  const btVector3 computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const;
+
+  // get a single node's all delta velocity
+  const btVector3 internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const;
+
+  //
+  // rigid motion related
+  //
+  void applyDamping(btScalar timeStep);
+
+  void applyCentralImpulse(const btVector3& impulse);
+
+  void applyTorqueImpulse(const btVector3& torque);
+
+  void proceedToTransform(btScalar dt, bool end_of_time_step);
+
+  //
+  // force related
+  //
+
+  // apply impulse to the rigid frame
+  void internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos);
+
+  // apply impulse to nodes in the full space
+  void internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt);
+
+  // apply nodal external force in the full space
+  void applyFullSpaceNodalForce(const btVector3& f_ext, int n_node);
+
+  // apply gravity to the rigid frame
+  void applyRigidGravity(const btVector3& gravity, btScalar dt);
+
+  // apply reduced elastic force
+  void applyReducedElasticForce(const tDenseArray& reduce_dofs);
+
+  // apply reduced damping force
+  void applyReducedDampingForce(const tDenseArray& reduce_vel);
+
+  // calculate the impulse factor
+  virtual btMatrix3x3 getImpulseFactor(int n_node);
+
+  // get relative position from a node to the CoM of the rigid frame
+  btVector3 getRelativePos(int n_node);
+
+  //
+  // accessors
+  //
+  bool isReducedModesOFF() const;
+  btScalar getTotalMass() const;
+  btTransform& getRigidTransform();
+  const btVector3& getLinearVelocity() const;
+	const btVector3& getAngularVelocity() const;
+
+  #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+  void clampVelocity(btVector3& v) const {
+      v.setX(
+          fmax(-BT_CLAMP_VELOCITY_TO,
+                fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
+      );
+      v.setY(
+          fmax(-BT_CLAMP_VELOCITY_TO,
+                fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
+      );
+      v.setZ(
+          fmax(-BT_CLAMP_VELOCITY_TO,
+                fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
+      );
+  }
+  #endif
+};
+
+#endif // BT_REDUCED_SOFT_BODY_H

thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp

@@ -0,0 +1,215 @@
+#include "btReducedDeformableBodyHelpers.h"
+#include "../btSoftBodyHelpers.h"
+#include <iostream>
+#include <string>
+#include <sstream>
+
+btReducedDeformableBody* btReducedDeformableBodyHelpers::createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only) {
+	std::string filename = file_path + vtk_file;
+	btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createFromVtkFile(worldInfo, filename.c_str());
+	
+	rsb->setReducedModes(num_modes, rsb->m_nodes.size());
+	btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(rsb, file_path.c_str());
+	
+	rsb->disableReducedModes(rigid_only);
+
+	return rsb;
+}
+
+btReducedDeformableBody* btReducedDeformableBodyHelpers::createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
+{
+	std::ifstream fs;
+	fs.open(vtk_file);
+	btAssert(fs);
+
+	typedef btAlignedObjectArray<int> Index;
+	std::string line;
+	btAlignedObjectArray<btVector3> X;
+	btVector3 position;
+	btAlignedObjectArray<Index> indices;
+	bool reading_points = false;
+	bool reading_tets = false;
+	size_t n_points = 0;
+	size_t n_tets = 0;
+	size_t x_count = 0;
+	size_t indices_count = 0;
+	while (std::getline(fs, line))
+	{
+		std::stringstream ss(line);
+		if (line.size() == (size_t)(0))
+		{
+		}
+		else if (line.substr(0, 6) == "POINTS")
+		{
+			reading_points = true;
+			reading_tets = false;
+			ss.ignore(128, ' ');  // ignore "POINTS"
+			ss >> n_points;
+			X.resize(n_points);
+		}
+		else if (line.substr(0, 5) == "CELLS")
+		{
+			reading_points = false;
+			reading_tets = true;
+			ss.ignore(128, ' ');  // ignore "CELLS"
+			ss >> n_tets;
+			indices.resize(n_tets);
+		}
+		else if (line.substr(0, 10) == "CELL_TYPES")
+		{
+			reading_points = false;
+			reading_tets = false;
+		}
+		else if (reading_points)
+		{
+			btScalar p;
+			ss >> p;
+			position.setX(p);
+			ss >> p;
+			position.setY(p);
+			ss >> p;
+			position.setZ(p);
+			//printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ());
+			X[x_count++] = position;
+		}
+		else if (reading_tets)
+		{
+			int d;
+			ss >> d;
+			if (d != 4)
+			{
+				printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
+				fs.close();
+				return 0;
+			}
+			ss.ignore(128, ' ');  // ignore "4"
+			Index tet;
+			tet.resize(4);
+			for (size_t i = 0; i < 4; i++)
+			{
+				ss >> tet[i];
+				//printf("%d ", tet[i]);
+			}
+			//printf("\n");
+			indices[indices_count++] = tet;
+		}
+	}
+	btReducedDeformableBody* rsb = new btReducedDeformableBody(&worldInfo, n_points, &X[0], 0);
+
+	for (int i = 0; i < n_tets; ++i)
+	{
+		const Index& ni = indices[i];
+		rsb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
+		{
+			rsb->appendLink(ni[0], ni[1], 0, true);
+			rsb->appendLink(ni[1], ni[2], 0, true);
+			rsb->appendLink(ni[2], ni[0], 0, true);
+			rsb->appendLink(ni[0], ni[3], 0, true);
+			rsb->appendLink(ni[1], ni[3], 0, true);
+			rsb->appendLink(ni[2], ni[3], 0, true);
+		}
+	}
+
+	btSoftBodyHelpers::generateBoundaryFaces(rsb);
+	rsb->initializeDmInverse();
+	rsb->m_tetraScratches.resize(rsb->m_tetras.size());
+	rsb->m_tetraScratchesTn.resize(rsb->m_tetras.size());
+	printf("Nodes:  %u\r\n", rsb->m_nodes.size());
+	printf("Links:  %u\r\n", rsb->m_links.size());
+	printf("Faces:  %u\r\n", rsb->m_faces.size());
+	printf("Tetras: %u\r\n", rsb->m_tetras.size());
+
+	fs.close();
+
+	return rsb;
+}
+
+void btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path)
+{
+	// read in eigenmodes, stiffness and mass matrices
+	std::string eigenvalues_file = std::string(file_path) + "eigenvalues.bin";
+	btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_eigenvalues, rsb->m_nReduced, eigenvalues_file.c_str());
+
+	std::string Kr_file = std::string(file_path) + "K_r_diag_mat.bin";
+	btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Kr,  rsb->m_nReduced, Kr_file.c_str());
+
+	// std::string Mr_file = std::string(file_path) + "M_r_diag_mat.bin";
+	// btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Mr, rsb->m_nReduced, Mr_file.c_str());
+
+	std::string modes_file = std::string(file_path) + "modes.bin";
+	btReducedDeformableBodyHelpers::readBinaryMat(rsb->m_modes, rsb->m_nReduced, 3 * rsb->m_nFull, modes_file.c_str());
+	
+	// read in full nodal mass
+	std::string M_file = std::string(file_path) + "M_diag_mat.bin";
+	btAlignedObjectArray<btScalar> mass_array;
+	btReducedDeformableBodyHelpers::readBinaryVec(mass_array, rsb->m_nFull, M_file.c_str());
+	rsb->setMassProps(mass_array);
+	
+	// calculate the inertia tensor in the local frame 
+ 	rsb->setInertiaProps();
+
+	// other internal initialization
+	rsb->internalInitialization();
+}
+
+// read in a vector from the binary file
+void btReducedDeformableBodyHelpers::readBinaryVec(btReducedDeformableBody::tDenseArray& vec, 
+																				  	 const unsigned int n_size, 				// #entries read
+																						 const char* file)
+{
+	std::ifstream f_in(file, std::ios::in | std::ios::binary);
+	// first get size
+	unsigned int size=0;
+	f_in.read((char*)&size, 4);//sizeof(unsigned int));
+	btAssert(size >= n_size); 	// make sure the #requested mode is smaller than the #available modes
+
+	// read data
+	vec.resize(n_size);
+	double temp;
+	for (unsigned int i = 0; i < n_size; ++i)
+	{
+		f_in.read((char*)&temp, sizeof(double));
+		vec[i] = btScalar(temp);
+	}
+  f_in.close();
+}
+
+// read in a matrix from the binary file
+void btReducedDeformableBodyHelpers::readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat, 
+																						 const unsigned int n_modes, 		// #modes, outer array size
+																						 const unsigned int n_full, 		// inner array size
+																						 const char* file)
+{
+	std::ifstream f_in(file, std::ios::in | std::ios::binary);
+	// first get size
+	unsigned int v_size=0;
+	f_in.read((char*)&v_size, 4);//sizeof(unsigned int));
+	btAssert(v_size >= n_modes * n_full); 	// make sure the #requested mode is smaller than the #available modes
+
+	// read data
+	mat.resize(n_modes);
+	for (int i = 0; i < n_modes; ++i) 
+	{
+		for (int j = 0; j < n_full; ++j)
+		{
+			double temp;
+			f_in.read((char*)&temp, sizeof(double));
+
+			if (mat[i].size() != n_modes)
+				mat[i].resize(n_full);
+			mat[i][j] = btScalar(temp);
+		}
+	}
+  f_in.close();
+}
+
+void btReducedDeformableBodyHelpers::calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin)
+{
+	btScalar lx = btScalar(2.) * (half_extents[0] + margin[0]);
+	btScalar ly = btScalar(2.) * (half_extents[1] + margin[1]);
+	btScalar lz = btScalar(2.) * (half_extents[2] + margin[2]);
+
+	inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
+								   mass / (btScalar(12.0)) * (lx * lx + lz * lz),
+								   mass / (btScalar(12.0)) * (lx * lx + ly * ly));
+}

thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h

@@ -0,0 +1,25 @@
+#ifndef BT_REDUCED_SOFT_BODY_HELPERS_H
+#define BT_REDUCED_SOFT_BODY_HELPERS_H
+
+#include "btReducedDeformableBody.h"
+#include <string>
+
+struct btReducedDeformableBodyHelpers
+{
+	// create a reduced deformable object
+	static btReducedDeformableBody* createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only);
+	// read in geometry info from Vtk file
+  static btReducedDeformableBody* createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
+	// read in all reduced files
+	static void readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path);
+	// read in a binary vector
+	static void readBinaryVec(btReducedDeformableBody::tDenseArray& vec, const unsigned int n_size, const char* file);
+	// read in a binary matrix
+	static void readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat, const unsigned int n_modes, const unsigned int n_full, const char* file);
+	
+	// calculate the local inertia tensor for a box shape reduced deformable object
+	static void calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin);
+};
+
+
+#endif // BT_REDUCED_SOFT_BODY_HELPERS_H

thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp

@@ -0,0 +1,325 @@
+#include "btReducedDeformableBodySolver.h"
+#include "btReducedDeformableBody.h"
+
+btReducedDeformableBodySolver::btReducedDeformableBodySolver()
+{
+  m_ascendOrder = true;
+  m_reducedSolver = true;
+  m_dampingAlpha = 0;
+  m_dampingBeta = 0;
+  m_gravity = btVector3(0, 0, 0);
+}
+
+void btReducedDeformableBodySolver::setGravity(const btVector3& gravity)
+{
+  m_gravity = gravity;
+}
+
+void btReducedDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt)
+{
+  m_softBodies.copyFromArray(bodies);
+	bool nodeUpdated = updateNodes();
+
+	if (nodeUpdated)
+	{
+		m_dv.resize(m_numNodes, btVector3(0, 0, 0));
+		m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
+		m_residual.resize(m_numNodes, btVector3(0, 0, 0));
+		m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
+	}
+
+	// need to setZero here as resize only set value for newly allocated items
+	for (int i = 0; i < m_numNodes; ++i)
+	{
+		m_dv[i].setZero();
+		m_ddv[i].setZero();
+		m_residual[i].setZero();
+	}
+
+	if (dt > 0)
+	{
+		m_dt = dt;
+	}
+	m_objective->reinitialize(nodeUpdated, dt);
+
+  int N = bodies.size();
+	if (nodeUpdated)
+	{
+		m_staticConstraints.resize(N);
+		m_nodeRigidConstraints.resize(N);
+		// m_faceRigidConstraints.resize(N);
+	}
+	for (int i = 0; i < N; ++i)
+	{
+		m_staticConstraints[i].clear();
+		m_nodeRigidConstraints[i].clear();
+		// m_faceRigidConstraints[i].clear();
+	}
+
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+    rsb->m_contactNodesList.clear();
+  }
+
+  // set node index offsets
+  int sum = 0;
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+    rsb->m_nodeIndexOffset = sum;
+    sum += rsb->m_nodes.size();
+  }
+
+	btDeformableBodySolver::updateSoftBodies();
+}
+
+void btReducedDeformableBodySolver::predictMotion(btScalar solverdt)
+{
+  applyExplicitForce(solverdt);
+
+  // predict new mesh location
+  predictReduceDeformableMotion(solverdt);
+
+  //TODO: check if there is anything missed from btDeformableBodySolver::predictDeformableMotion
+}
+
+void btReducedDeformableBodySolver::predictReduceDeformableMotion(btScalar solverdt)
+{
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+    if (!rsb->isActive())
+    {
+      continue;
+    }
+
+    // clear contacts variables
+		rsb->m_nodeRigidContacts.resize(0);
+		rsb->m_faceRigidContacts.resize(0);
+		rsb->m_faceNodeContacts.resize(0);
+    
+    // calculate inverse mass matrix for all nodes
+    for (int j = 0; j < rsb->m_nodes.size(); ++j)
+    {
+      if (rsb->m_nodes[j].m_im > 0)
+      {
+        rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse();
+      }
+    }
+
+    // rigid motion: t, R at time^*
+    rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform());
+
+    // update reduced dofs at time^*
+    // rsb->updateReducedDofs(solverdt);
+
+    // update local moment arm at time^*
+    // rsb->updateLocalMomentArm();
+    // rsb->updateExternalForceProjectMatrix(true);
+
+    // predict full space velocity at time^* (needed for constraints)
+    rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform());
+
+    // update full space nodal position at time^*
+    rsb->mapToFullPosition(rsb->getInterpolationWorldTransform());
+
+    // update bounding box
+    rsb->updateBounds();
+
+    // update tree
+    rsb->updateNodeTree(true, true);
+    if (!rsb->m_fdbvt.empty())
+    {
+      rsb->updateFaceTree(true, true);
+    }
+  }
+}
+
+void btReducedDeformableBodySolver::applyExplicitForce(btScalar solverdt)
+{
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+
+    // apply gravity to the rigid frame, get m_linearVelocity at time^*
+    rsb->applyRigidGravity(m_gravity, solverdt);
+
+    if (!rsb->isReducedModesOFF())
+    {
+      // add internal force (elastic force & damping force)
+      rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer);
+      rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
+
+      // get reduced velocity at time^* 
+      rsb->updateReducedVelocity(solverdt);
+    }
+
+    // apply damping (no need at this point)
+    // rsb->applyDamping(solverdt);
+  }
+}
+
+void btReducedDeformableBodySolver::applyTransforms(btScalar timeStep)
+{
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+
+    // rigid motion
+    rsb->proceedToTransform(timeStep, true);
+
+    if (!rsb->isReducedModesOFF())
+    {
+      // update reduced dofs for time^n+1
+      rsb->updateReducedDofs(timeStep);
+
+      // update local moment arm for time^n+1
+      rsb->updateLocalMomentArm();
+      rsb->updateExternalForceProjectMatrix(true);
+    }
+
+    // update mesh nodal positions for time^n+1
+    rsb->mapToFullPosition(rsb->getRigidTransform());
+
+    // update mesh nodal velocity
+    rsb->mapToFullVelocity(rsb->getRigidTransform());
+
+    // end of time step clean up and update
+    rsb->endOfTimeStepZeroing();
+
+    // update the rendering mesh
+    rsb->interpolateRenderMesh();
+  }
+}
+
+void btReducedDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
+{
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+    if (!rsb->isActive())
+		{
+			continue;
+		}
+
+    // set fixed constraints
+    for (int j = 0; j < rsb->m_fixedNodes.size(); ++j)
+		{
+      int i_node = rsb->m_fixedNodes[j];
+			if (rsb->m_nodes[i_node].m_im == 0)
+			{
+        for (int k = 0; k < 3; ++k)
+        {
+          btVector3 dir(0, 0, 0);
+          dir[k] = 1;
+          btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], dir, infoGlobal, m_dt);
+          m_staticConstraints[i].push_back(static_constraint);
+        }
+			}
+		}
+    btAssert(rsb->m_fixedNodes.size() * 3 == m_staticConstraints[i].size());
+
+    // set Deformable Node vs. Rigid constraint
+		for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j)
+		{
+			const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j];
+			// skip fixed points
+			if (contact.m_node->m_im == 0)
+			{
+				continue;
+			}
+			btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt);
+			m_nodeRigidConstraints[i].push_back(constraint);
+      rsb->m_contactNodesList.push_back(contact.m_node->index - rsb->m_nodeIndexOffset);
+		}
+    // std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n";
+    // std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n";
+
+  }
+}
+
+btScalar btReducedDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+{
+  btScalar residualSquare = 0;
+
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btAlignedObjectArray<int> m_orderNonContactConstraintPool;
+    btAlignedObjectArray<int> m_orderContactConstraintPool;
+
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+
+    // shuffle the order of applying constraint
+    m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size());
+    m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size());
+    if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+    {
+      // fixed constraint order
+      for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+      {
+        m_orderNonContactConstraintPool[j] = m_ascendOrder ? j : m_staticConstraints[i].size() - 1 - j;
+      }
+      // contact constraint order
+      for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+      {
+        m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j;
+      }
+
+      m_ascendOrder = m_ascendOrder ? false : true;
+    }
+    else
+    {
+      for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+      {
+        m_orderNonContactConstraintPool[j] = j;
+      }
+      // contact constraint order
+      for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+      {
+        m_orderContactConstraintPool[j] = j;
+      }
+    }
+
+    // handle fixed constraint
+    for (int k = 0; k < m_staticConstraints[i].size(); ++k)
+    {
+      btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][m_orderNonContactConstraintPool[k]];
+      btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+      residualSquare = btMax(residualSquare, localResidualSquare);
+    }
+
+    // handle contact constraint
+
+    // node vs rigid contact
+    // std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n';
+    for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k)
+    {
+      btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][m_orderContactConstraintPool[k]];
+      btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+      residualSquare = btMax(residualSquare, localResidualSquare);
+    }
+
+    // face vs rigid contact
+    // for (int k = 0; k < m_faceRigidConstraints[i].size(); ++k)
+    // {
+    // 	btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][k];
+    // 	btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+    // 	residualSquare = btMax(residualSquare, localResidualSquare);
+    // }
+  }
+
+  
+	return residualSquare;
+}
+
+void btReducedDeformableBodySolver::deformableBodyInternalWriteBack()
+{
+  // reduced deformable update
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+    rsb->applyInternalVelocityChanges();
+  }
+  m_ascendOrder = true;
+}

thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h

@@ -0,0 +1,61 @@
+#ifndef BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H
+#define BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H
+
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "btReducedDeformableContactConstraint.h"
+
+class btReducedDeformableBody;
+
+class btReducedDeformableBodySolver : public btDeformableBodySolver
+{
+ protected:
+  bool m_ascendOrder;
+  btScalar m_dampingAlpha;
+  btScalar m_dampingBeta;
+
+  btVector3 m_gravity;
+
+  void predictReduceDeformableMotion(btScalar solverdt);
+
+  void applyExplicitForce(btScalar solverdt);
+
+ public:
+  btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableStaticConstraint> > m_staticConstraints;
+  btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
+  btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
+  
+  btReducedDeformableBodySolver();
+  ~btReducedDeformableBodySolver() {}
+
+  virtual void setGravity(const btVector3& gravity);
+
+  virtual SolverTypes getSolverType() const
+  {
+    return REDUCED_DEFORMABLE_SOLVER;
+  }
+
+  // resize/clear data structures
+	virtual void reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt);
+
+  virtual void predictMotion(btScalar solverdt);
+
+  virtual void applyTransforms(btScalar timeStep);
+
+  // set up contact constraints
+	virtual void setConstraints(const btContactSolverInfo& infoGlobal);
+
+  // solve all constraints (fixed and contact)
+  virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
+  // apply all the delta velocities
+  virtual void deformableBodyInternalWriteBack();
+
+  // virtual void setProjection() {}
+
+  // virtual void setLagrangeMultiplier() {}
+
+  // virtual void setupDeformableSolve(bool implicit);
+
+};
+
+#endif // BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H

thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp

@@ -0,0 +1,579 @@
+#include "btReducedDeformableContactConstraint.h"
+#include <iostream>
+
+// ================= static constraints ===================
+btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint(
+  btReducedDeformableBody* rsb, 
+  btSoftBody::Node* node,
+	const btVector3& ri,
+	const btVector3& x0,
+	const btVector3& dir,
+  const btContactSolverInfo& infoGlobal,
+	btScalar dt)
+  : m_rsb(rsb), m_ri(ri), m_targetPos(x0), m_impulseDirection(dir), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
+{
+	m_erp = 0.2;
+	m_appliedImpulse = 0;
+
+	// get impulse factor
+  m_impulseFactorMatrix = rsb->getImpulseFactor(m_node->index);
+	m_impulseFactor = (m_impulseFactorMatrix * m_impulseDirection).dot(m_impulseDirection);
+
+	btScalar vel_error = btDot(-m_node->m_v, m_impulseDirection);
+	btScalar pos_error = btDot(m_targetPos - m_node->m_x, m_impulseDirection);
+
+	m_rhs = (vel_error + m_erp * pos_error / m_dt) / m_impulseFactor;
+}
+
+btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
+{
+	// target velocity of fixed constraint is 0
+	btVector3 deltaVa = getDeltaVa();
+	btScalar deltaV_rel = btDot(deltaVa, m_impulseDirection);
+  btScalar deltaImpulse = m_rhs - deltaV_rel / m_impulseFactor;
+	m_appliedImpulse = m_appliedImpulse + deltaImpulse;
+
+	btVector3 impulse = deltaImpulse * m_impulseDirection;
+	applyImpulse(impulse);
+
+	// calculate residual
+	btScalar residualSquare = m_impulseFactor * deltaImpulse;
+	residualSquare *= residualSquare;
+
+	return residualSquare;
+}
+  
+// this calls reduced deformable body's internalApplyFullSpaceImpulse
+void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse)
+{
+	// apply full space impulse
+	m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
+}
+
+btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const
+{
+	return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
+}
+
+// ================= base contact constraints ===================
+btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint(
+  btReducedDeformableBody* rsb, 
+  const btSoftBody::DeformableRigidContact& c, 
+  const btContactSolverInfo& infoGlobal,
+	btScalar dt)
+  : m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal)
+{
+	m_nodeQueryIndex = 0;
+	m_appliedNormalImpulse = 0;
+  m_appliedTangentImpulse = 0;
+	m_rhs = 0;
+	m_rhs_tangent = 0;
+	m_cfm = infoGlobal.m_deformable_cfm;
+	m_cfm_friction = 0;
+	m_erp = infoGlobal.m_deformable_erp;
+	m_erp_friction = infoGlobal.m_deformable_erp;
+	m_friction = infoGlobal.m_friction;
+
+	m_collideStatic = m_contact->m_cti.m_colObj->isStaticObject();
+	m_collideMultibody = (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK);
+}
+
+void btReducedDeformableRigidContactConstraint::setSolverBody(const int bodyId, btSolverBody& solver_body)
+{
+	if (!m_collideMultibody)
+	{
+		m_solverBodyId = bodyId;
+		m_solverBody = &solver_body;
+		m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass();
+		btVector3	torqueAxis = -m_relPosA.cross(m_contactNormalA);
+		m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
+		
+		m_linearComponentTangent = m_contactTangent * m_solverBody->internalGetInvMass();
+		btVector3 torqueAxisTangent = m_relPosA.cross(m_contactTangent);
+		m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent;
+	}
+}
+
+btVector3 btReducedDeformableRigidContactConstraint::getVa() const
+{
+	btVector3 Va(0, 0, 0);
+	if (!m_collideStatic)
+	{
+		Va = btDeformableRigidContactConstraint::getVa();
+	}
+	return Va;
+}
+
+btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
+{
+	// btVector3 Va = getVa();
+	// btVector3 deltaVa = Va - m_bufferVelocityA;
+	// if (!m_collideStatic)
+	// {
+		// std::cout << "moving collision!!!\n";
+		// std::cout << "relPosA: " << m_relPosA[0] << "\t" << m_relPosA[1] << "\t" << m_relPosA[2] << "\n";
+		// std::cout << "moving rigid linear_vel: " << m_solverBody->m_originalBody->getLinearVelocity()[0] << '\t'
+		//  << m_solverBody->m_originalBody->getLinearVelocity()[1] << '\t'
+		//   << m_solverBody->m_originalBody->getLinearVelocity()[2] << '\n';
+	// }
+	btVector3 deltaVa = getDeltaVa();
+	btVector3 deltaVb = getDeltaVb();
+
+	// if (!m_collideStatic)
+	// {
+	// 	std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n';
+	// 	std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n';
+	// }
+
+	// get delta relative velocity and magnitude (i.e., how much impulse has been applied?)
+	btVector3 deltaV_rel = deltaVa - deltaVb;
+	btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA);
+
+	// if (!m_collideStatic)
+	// {
+	// 	std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n";
+	// 	std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n";
+	// 	std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n';
+	// }
+	
+	// get the normal impulse to be applied
+	btScalar deltaImpulse = m_rhs - m_appliedNormalImpulse * m_cfm - deltaV_rel_normal / m_normalImpulseFactor;
+	// if (!m_collideStatic)
+	// {
+	// 	std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: "  << m_appliedNormalImpulse << "\n";
+	// 	std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n';
+	// }
+
+	{
+		// cumulative impulse that has been applied
+		btScalar sum = m_appliedNormalImpulse + deltaImpulse;
+		// if the cumulative impulse is pushing the object into the rigid body, set it zero
+		if (sum < 0)
+		{
+			deltaImpulse = -m_appliedNormalImpulse;
+			m_appliedNormalImpulse = 0;
+		}
+		else
+		{
+			m_appliedNormalImpulse = sum;
+		}	
+	}
+
+	// if (!m_collideStatic)
+	// {
+	// 	std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n';
+	// 	std::cout << "deltaImpulse: " << deltaImpulse << '\n';
+	// }
+
+	// residual is the nodal normal velocity change in current iteration
+	btScalar residualSquare = deltaImpulse * m_normalImpulseFactor;	// get residual
+	residualSquare *= residualSquare;
+
+	
+	// apply Coulomb friction (based on delta velocity, |dv_t| = |dv_n * friction|)
+	btScalar deltaImpulse_tangent = 0;
+	btScalar deltaImpulse_tangent2 = 0;
+	{
+		// calculate how much impulse is needed
+		// btScalar deltaV_rel_tangent = btDot(deltaV_rel, m_contactTangent);
+		// btScalar impulse_changed = deltaV_rel_tangent * m_tangentImpulseFactorInv;
+		// deltaImpulse_tangent = m_rhs_tangent - impulse_changed;
+
+		// btScalar sum = m_appliedTangentImpulse + deltaImpulse_tangent;
+		btScalar lower_limit = - m_appliedNormalImpulse * m_friction;
+		btScalar upper_limit = m_appliedNormalImpulse * m_friction;
+		// if (sum > upper_limit)
+		// {
+		// 	deltaImpulse_tangent = upper_limit - m_appliedTangentImpulse;
+		// 	m_appliedTangentImpulse = upper_limit;
+		// }
+		// else if (sum < lower_limit)
+		// {
+		// 	deltaImpulse_tangent = lower_limit - m_appliedTangentImpulse;
+		// 	m_appliedTangentImpulse = lower_limit;
+		// }
+		// else
+		// {
+		// 	m_appliedTangentImpulse = sum;
+		// }
+		// 
+		calculateTangentialImpulse(deltaImpulse_tangent, m_appliedTangentImpulse, m_rhs_tangent,
+															 m_tangentImpulseFactorInv, m_contactTangent, lower_limit, upper_limit, deltaV_rel);
+		
+		if (m_collideMultibody)
+		{
+			calculateTangentialImpulse(deltaImpulse_tangent2, m_appliedTangentImpulse2, m_rhs_tangent2,
+															   m_tangentImpulseFactorInv2, m_contactTangent2, lower_limit, upper_limit, deltaV_rel);
+		}
+															 
+
+		if (!m_collideStatic)
+		{
+			// std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t"  << m_contactTangent[1] << "\t"  << m_contactTangent[2] << "\n";
+			// std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << '\n';
+			// std::cout << "deltaImpulseTangent: " << deltaImpulse_tangent << '\n';
+			// std::cout << "m_appliedTangentImpulse: " << m_appliedTangentImpulse << '\n';
+		}
+	}
+
+	// get the total impulse vector
+	btVector3 impulse_normal = deltaImpulse * m_contactNormalA;
+	btVector3 impulse_tangent = deltaImpulse_tangent * (-m_contactTangent);
+	btVector3 impulse_tangent2 = deltaImpulse_tangent2 * (-m_contactTangent2);
+	btVector3 impulse = impulse_normal + impulse_tangent + impulse_tangent2;
+
+	applyImpulse(impulse);
+	
+	// apply impulse to the rigid/multibodies involved and change their velocities
+	if (!m_collideStatic)
+	{
+		// std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t'
+		// 																	<< m_linearComponentNormal[1] << '\t'
+		// 																	<< m_linearComponentNormal[2] << '\n';
+		// std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t'
+		// 																	<< m_angularComponentNormal[1] << '\t'
+		// 																	<< m_angularComponentNormal[2] << '\n';
+
+		if (!m_collideMultibody)		// collision with rigid body
+		{
+			// std::cout << "rigid impulse applied!!\n";
+			// std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
+			// << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
+			// 	<< m_solverBody->getDeltaLinearVelocity()[2] << '\n';
+			// std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
+			// << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
+			// 	<< m_solverBody->getDeltaAngularVelocity()[2] << '\n';
+
+			m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse);
+			m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent);
+
+			// std::cout << "after\n";
+			// std::cout << "rigid impulse applied!!\n";
+			// std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
+			// << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
+			// 	<< m_solverBody->getDeltaLinearVelocity()[2] << '\n';
+			// std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
+			// << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
+			// 	<< m_solverBody->getDeltaAngularVelocity()[2] << '\n';
+		}
+		else		// collision with multibody
+		{
+			btMultiBodyLinkCollider* multibodyLinkCol = 0;
+			multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
+			if (multibodyLinkCol)
+			{
+				const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+				// apply normal component of the impulse
+				multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, -deltaImpulse);
+				
+				// const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+				// std::cout << "deltaV_normal: ";
+				// for (int i = 0; i < ndof; ++i)
+				// {
+				// 	std::cout << i << "\t" << deltaV_normal[i] << '\n';
+				// }
+
+				if (impulse_tangent.norm() > SIMD_EPSILON)
+				{
+					// apply tangential component of the impulse
+					const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+					multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, deltaImpulse_tangent);
+					const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+					multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, deltaImpulse_tangent2);
+				}
+			}
+		}
+	}
+	return residualSquare;
+}
+
+void btReducedDeformableRigidContactConstraint::calculateTangentialImpulse(btScalar& deltaImpulse_tangent, 
+																		 																			 btScalar& appliedImpulse, 
+																																					 const btScalar rhs_tangent,
+																																					 const btScalar tangentImpulseFactorInv,
+																																					 const btVector3& tangent,
+																		 																			 const btScalar lower_limit,
+																																					 const btScalar upper_limit,
+																																					 const btVector3& deltaV_rel)
+{
+	btScalar deltaV_rel_tangent = btDot(deltaV_rel, tangent);
+	btScalar impulse_changed = deltaV_rel_tangent * tangentImpulseFactorInv;
+	deltaImpulse_tangent = rhs_tangent - m_cfm_friction * appliedImpulse - impulse_changed;
+
+	btScalar sum = appliedImpulse + deltaImpulse_tangent;
+	if (sum > upper_limit)
+	{
+		deltaImpulse_tangent = upper_limit - appliedImpulse;
+		appliedImpulse = upper_limit;
+	}
+	else if (sum < lower_limit)
+	{
+		deltaImpulse_tangent = lower_limit - appliedImpulse;
+		appliedImpulse = lower_limit;
+	}
+	else
+	{
+		appliedImpulse = sum;
+	}
+}
+
+// ================= node vs rigid constraints ===================
+btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidContactConstraint(
+  btReducedDeformableBody* rsb, 
+  const btSoftBody::DeformableNodeRigidContact& contact, 
+  const btContactSolverInfo& infoGlobal,
+	btScalar dt)
+  : m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
+{
+	m_contactNormalA = contact.m_cti.m_normal;
+  m_contactNormalB = -contact.m_cti.m_normal;
+
+	if (contact.m_node->index < rsb->m_nodes.size())
+	{
+		m_nodeQueryIndex = contact.m_node->index;
+	}
+	else
+	{
+		m_nodeQueryIndex = m_node->index - rsb->m_nodeIndexOffset;
+	}
+
+	if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+	{
+		m_relPosA = contact.m_c1;
+	}
+	else
+	{
+		m_relPosA = btVector3(0,0,0);
+	}
+	m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin();
+
+	if (m_collideStatic)		// colliding with static object, only consider reduced deformable body's impulse factor
+	{
+		m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex);
+	}
+	else		// colliding with dynamic object, consider both reduced deformable and rigid body's impulse factors
+	{
+		m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex) + contact.m_c0;
+	}
+
+	m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA);
+	m_tangentImpulseFactor = 0;
+
+	warmStarting();
+}
+
+void btReducedDeformableNodeRigidContactConstraint::warmStarting()
+{
+	btVector3 va = getVa();
+	btVector3 vb = getVb();
+	m_bufferVelocityA = va;
+	m_bufferVelocityB = vb;
+
+	// we define the (+) direction of errors to be the outward surface normal of the rigid object
+	btVector3 v_rel = vb - va;
+	// get tangent direction of the relative velocity
+	btVector3 v_tangent = v_rel - v_rel.dot(m_contactNormalA) * m_contactNormalA;
+	if (v_tangent.norm() < SIMD_EPSILON)
+	{
+		m_contactTangent = btVector3(0, 0, 0);
+		// tangent impulse factor
+		m_tangentImpulseFactor = 0;
+		m_tangentImpulseFactorInv = 0;
+	}
+	else
+	{
+		if (!m_collideMultibody)
+		{
+			m_contactTangent = v_tangent.normalized();
+			m_contactTangent2.setZero();
+			// tangent impulse factor 1
+			m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
+			m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
+			// tangent impulse factor 2
+			m_tangentImpulseFactor2 = 0;
+			m_tangentImpulseFactorInv2 = 0;
+		}
+		else	// multibody requires 2 contact directions
+		{
+			m_contactTangent = m_contact->t1;
+			m_contactTangent2 = m_contact->t2;
+
+			// tangent impulse factor 1
+			m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
+			m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
+			// tangent impulse factor 2
+			m_tangentImpulseFactor2 = (m_impulseFactor * m_contactTangent2).dot(m_contactTangent2);
+			m_tangentImpulseFactorInv2 = btScalar(1) / m_tangentImpulseFactor2;
+		}
+	}
+
+
+	// initial guess for normal impulse
+	{
+		btScalar velocity_error = btDot(v_rel, m_contactNormalA);	// magnitude of relative velocity
+		btScalar position_error = 0;
+		if (m_penetration > 0)
+		{
+			velocity_error += m_penetration / m_dt;
+		}
+		else
+		{
+			// add penetration correction vel
+			position_error = m_penetration * m_erp / m_dt;
+		}
+		// get the initial estimate of impulse magnitude to be applied
+		m_rhs = -(velocity_error + position_error) / m_normalImpulseFactor;
+	}
+
+	// initial guess for tangential impulse
+	{
+		btScalar velocity_error = btDot(v_rel, m_contactTangent);
+		m_rhs_tangent = velocity_error * m_tangentImpulseFactorInv;
+
+		if (m_collideMultibody)
+		{
+			btScalar velocity_error2 = btDot(v_rel, m_contactTangent2);
+			m_rhs_tangent2 = velocity_error2 * m_tangentImpulseFactorInv2;
+		}
+	}
+
+	// warm starting
+	// applyImpulse(m_rhs * m_contactNormalA);
+	// if (!m_collideStatic)
+	// {
+	// 	const btSoftBody::sCti& cti = m_contact->m_cti;
+	// 	if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+	// 	{
+	// 		m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -m_rhs);
+	// 	}
+	// }
+}
+
+btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const
+{
+	return m_node->m_v;
+}
+
+btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const
+{
+	btVector3 deltaVa(0, 0, 0);
+	if (!m_collideStatic)
+	{
+		if (!m_collideMultibody)		// for rigid body
+		{
+			deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_solverBody->internalGetDeltaAngularVelocity().cross(m_relPosA);
+		}
+		else		// for multibody
+		{
+			btMultiBodyLinkCollider* multibodyLinkCol = 0;
+			multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
+			if (multibodyLinkCol)
+			{
+				const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+				const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
+				const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
+				const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
+				const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
+				// add in the normal component of the va
+				btScalar vel = 0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_dv[k] * J_n[k];
+				}
+				deltaVa = m_contact->m_cti.m_normal * vel;
+				
+				// add in the tangential components of the va
+				vel = 0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_dv[k] * J_t1[k];
+				}
+				deltaVa += m_contact->t1 * vel;
+
+				vel = 0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_dv[k] * J_t2[k];
+				}
+				deltaVa += m_contact->t2 * vel;
+			}
+		}
+	}
+	return deltaVa;
+}
+
+btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const
+{	
+	// std::cout << "node: " << m_node->index << '\n';
+	return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_nodeQueryIndex);
+}
+
+btVector3 btReducedDeformableNodeRigidContactConstraint::getSplitVb() const
+{
+	return m_node->m_splitv;
+}
+
+btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+	return m_total_normal_dv + m_total_tangent_dv;
+}
+
+void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
+{
+  m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_nodeQueryIndex, m_dt);
+	// m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt);
+	// m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
+	// if (!m_collideStatic)
+	// {
+	// 	// std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
+	// 	// std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n';
+	// 	btVector3 v_after = getDeltaVb() + m_node->m_v;
+	// 	// std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n';
+	// }
+	// std::cout << "node: " << m_node->index << " pos: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n';
+}
+
+// ================= face vs rigid constraints ===================
+btReducedDeformableFaceRigidContactConstraint::btReducedDeformableFaceRigidContactConstraint(
+  btReducedDeformableBody* rsb, 
+  const btSoftBody::DeformableFaceRigidContact& contact, 
+  const btContactSolverInfo& infoGlobal,
+	btScalar dt, 
+  bool useStrainLimiting)
+  : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
+{}
+
+btVector3 btReducedDeformableFaceRigidContactConstraint::getVb() const
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
+	return vb;
+}
+
+btVector3 btReducedDeformableFaceRigidContactConstraint::getSplitVb() const
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2];
+	return vb;
+}
+
+btVector3 btReducedDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+	btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	if (m_face->m_n[0] == node)
+	{
+		return face_dv * contact->m_weights[0];
+	}
+	if (m_face->m_n[1] == node)
+	{
+		return face_dv * contact->m_weights[1];
+	}
+	btAssert(node == m_face->m_n[2]);
+	return face_dv * contact->m_weights[2];
+}
+
+void btReducedDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
+{
+  //
+}

thirdparty/bullet/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h

@@ -0,0 +1,194 @@
+#include "../btDeformableContactConstraint.h"
+#include "btReducedDeformableBody.h"
+
+// ================= static constraints ===================
+class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint
+{
+ public:
+  btReducedDeformableBody* m_rsb;
+  btScalar m_dt;
+  btVector3 m_ri;
+  btVector3 m_targetPos;
+  btVector3 m_impulseDirection;
+  btMatrix3x3 m_impulseFactorMatrix;
+  btScalar m_impulseFactor;
+  btScalar m_rhs;
+  btScalar m_appliedImpulse;
+  btScalar m_erp;
+
+  btReducedDeformableStaticConstraint(btReducedDeformableBody* rsb, 
+                                      btSoftBody::Node* node,
+                                      const btVector3& ri,
+                                      const btVector3& x0,
+                                      const btVector3& dir,
+                                      const btContactSolverInfo& infoGlobal,
+                                      btScalar dt);
+	// btReducedDeformableStaticConstraint(const btReducedDeformableStaticConstraint& other);
+  btReducedDeformableStaticConstraint() {}
+  virtual ~btReducedDeformableStaticConstraint() {}
+
+  virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+  
+  // this calls reduced deformable body's applyFullSpaceImpulse
+  virtual void applyImpulse(const btVector3& impulse);
+
+  btVector3 getDeltaVa() const;
+
+  // virtual void applySplitImpulse(const btVector3& impulse) {}
+};
+
+// ================= base contact constraints ===================
+class btReducedDeformableRigidContactConstraint : public btDeformableRigidContactConstraint
+{
+ public:
+  bool m_collideStatic;     // flag for collision with static object
+  bool m_collideMultibody;  // flag for collision with multibody
+
+  int m_nodeQueryIndex;
+  int m_solverBodyId;       // for debugging
+
+  btReducedDeformableBody* m_rsb;
+  btSolverBody* m_solverBody;
+  btScalar m_dt;
+
+  btScalar m_appliedNormalImpulse;
+  btScalar m_appliedTangentImpulse;
+  btScalar m_appliedTangentImpulse2;
+  btScalar m_normalImpulseFactor;
+  btScalar m_tangentImpulseFactor;
+  btScalar m_tangentImpulseFactor2;
+  btScalar m_tangentImpulseFactorInv;
+  btScalar m_tangentImpulseFactorInv2;
+  btScalar m_rhs;
+  btScalar m_rhs_tangent;
+  btScalar m_rhs_tangent2;
+  
+  btScalar m_cfm;
+  btScalar m_cfm_friction;
+  btScalar m_erp;
+  btScalar m_erp_friction;
+  btScalar m_friction;
+
+  btVector3 m_contactNormalA;     // surface normal for rigid body (opposite direction as impulse)
+  btVector3 m_contactNormalB;     // surface normal for reduced deformable body (opposite direction as impulse)
+  btVector3 m_contactTangent;     // tangential direction of the relative velocity
+  btVector3 m_contactTangent2;    // 2nd tangential direction of the relative velocity
+  btVector3 m_relPosA;            // relative position of the contact point for A (rigid)
+  btVector3 m_relPosB;            // relative position of the contact point for B
+  btMatrix3x3 m_impulseFactor;    // total impulse matrix
+
+  btVector3 m_bufferVelocityA;    // velocity at the beginning of the iteration
+  btVector3 m_bufferVelocityB;
+  btVector3 m_linearComponentNormal;    // linear components for the solver body
+  btVector3 m_angularComponentNormal;   // angular components for the solver body
+  // since 2nd contact direction only applies to multibody, these components will never be used
+  btVector3 m_linearComponentTangent;
+  btVector3 m_angularComponentTangent;
+
+  btReducedDeformableRigidContactConstraint(btReducedDeformableBody* rsb, 
+                                            const btSoftBody::DeformableRigidContact& c, 
+                                            const btContactSolverInfo& infoGlobal,
+                                            btScalar dt);
+	// btReducedDeformableRigidContactConstraint(const btReducedDeformableRigidContactConstraint& other);
+  btReducedDeformableRigidContactConstraint() {}
+  virtual ~btReducedDeformableRigidContactConstraint() {}
+
+  void setSolverBody(const int bodyId, btSolverBody& solver_body);
+
+  virtual void warmStarting() {}
+
+  virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+
+  void calculateTangentialImpulse(btScalar& deltaImpulse_tangent, 
+                                  btScalar& appliedImpulse, 
+                                  const btScalar rhs_tangent,
+                                  const btScalar tangentImpulseFactorInv,
+                                  const btVector3& tangent,
+                                  const btScalar lower_limit,
+                                  const btScalar upper_limit,
+                                  const btVector3& deltaV_rel);
+
+  virtual void applyImpulse(const btVector3& impulse) {}
+
+  virtual void applySplitImpulse(const btVector3& impulse) {} // TODO: may need later
+
+  virtual btVector3 getVa() const;
+  virtual btVector3 getDeltaVa() const = 0;
+  virtual btVector3 getDeltaVb() const = 0;
+};
+
+// ================= node vs rigid constraints ===================
+class btReducedDeformableNodeRigidContactConstraint : public btReducedDeformableRigidContactConstraint
+{
+ public:
+  btSoftBody::Node* m_node;
+
+  btReducedDeformableNodeRigidContactConstraint(btReducedDeformableBody* rsb, 
+                                                const btSoftBody::DeformableNodeRigidContact& contact, 
+                                                const btContactSolverInfo& infoGlobal,
+                                                btScalar dt);
+	// btReducedDeformableNodeRigidContactConstraint(const btReducedDeformableNodeRigidContactConstraint& other);
+  btReducedDeformableNodeRigidContactConstraint() {}
+  virtual ~btReducedDeformableNodeRigidContactConstraint() {}
+
+  virtual void warmStarting();
+
+  // get the velocity of the deformable node in contact
+	virtual btVector3 getVb() const;
+
+  // get the velocity change of the rigid body
+  virtual btVector3 getDeltaVa() const;
+
+  // get velocity change of the node in contat
+  virtual btVector3 getDeltaVb() const;
+
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const;
+
+	// get the velocity change of the input soft body node in the constraint
+	virtual btVector3 getDv(const btSoftBody::Node*) const;
+
+	// cast the contact to the desired type
+	const btSoftBody::DeformableNodeRigidContact* getContact() const
+	{
+		return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
+	}
+  
+  // this calls reduced deformable body's applyFullSpaceImpulse
+  virtual void applyImpulse(const btVector3& impulse);
+};
+
+// ================= face vs rigid constraints ===================
+class btReducedDeformableFaceRigidContactConstraint : public btReducedDeformableRigidContactConstraint
+{
+ public:
+  btSoftBody::Face* m_face;
+	bool m_useStrainLimiting;
+
+  btReducedDeformableFaceRigidContactConstraint(btReducedDeformableBody* rsb, 
+                                                const btSoftBody::DeformableFaceRigidContact& contact, 
+                                                const btContactSolverInfo& infoGlobal,
+                                                btScalar dt, 
+                                                bool useStrainLimiting);
+	// btReducedDeformableFaceRigidContactConstraint(const btReducedDeformableFaceRigidContactConstraint& other);
+  btReducedDeformableFaceRigidContactConstraint() {}
+  virtual ~btReducedDeformableFaceRigidContactConstraint() {}
+
+  // get the velocity of the deformable face at the contact point
+	virtual btVector3 getVb() const;
+
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const;
+
+	// get the velocity change of the input soft body node in the constraint
+	virtual btVector3 getDv(const btSoftBody::Node*) const;
+
+	// cast the contact to the desired type
+	const btSoftBody::DeformableFaceRigidContact* getContact() const
+	{
+		return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
+	}
+
+  // this calls reduced deformable body's applyFullSpaceImpulse
+  virtual void applyImpulse(const btVector3& impulse);
+};

thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -25,12 +25,18 @@
 #include "btDeformableNeoHookeanForce.h"
 #include "btDeformableContactProjection.h"
 #include "btPreconditioner.h"
-#include "btDeformableMultiBodyDynamicsWorld.h"
+// #include "btDeformableMultiBodyDynamicsWorld.h"
 #include "LinearMath/btQuickprof.h"
 
 class btDeformableBackwardEulerObjective
 {
 public:
+	enum _
+	{
+		Mass_preconditioner,
+		KKT_preconditioner
+	};
+
 	typedef btAlignedObjectArray<btVector3> TVStack;
 	btScalar m_dt;
 	btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;

thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp

@@ -23,6 +23,7 @@ btDeformableBodySolver::btDeformableBodySolver()
 	: m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
 {
 	m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
+	m_reducedSolver = false;
 }
 
 btDeformableBodySolver::~btDeformableBodySolver()
@@ -401,7 +402,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt)
 			}
 		}
 	}
-	m_objective->applyExplicitForce(m_residual);
+	applyExplicitForce();
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		btSoftBody* psb = m_softBodies[i];
@@ -411,6 +412,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt)
 			psb->m_nodeRigidContacts.resize(0);
 			psb->m_faceRigidContacts.resize(0);
 			psb->m_faceNodeContacts.resize(0);
+			psb->m_faceNodeContactsCCD.resize(0);
 			// predict motion for collision detection
 			predictDeformableMotion(psb, solverdt);
 		}
@@ -503,3 +505,89 @@ void btDeformableBodySolver::setLineSearch(bool lineSearch)
 {
 	m_lineSearch = lineSearch;
 }
+
+void btDeformableBodySolver::applyExplicitForce()
+{
+	m_objective->applyExplicitForce(m_residual);
+}
+
+void btDeformableBodySolver::applyTransforms(btScalar timeStep)
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			btSoftBody::Node& node = psb->m_nodes[j];
+			btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
+			btScalar clampDeltaV = maxDisplacement / timeStep;
+			for (int c = 0; c < 3; c++)
+			{
+				if (node.m_v[c] > clampDeltaV)
+				{
+					node.m_v[c] = clampDeltaV;
+				}
+				if (node.m_v[c] < -clampDeltaV)
+				{
+					node.m_v[c] = -clampDeltaV;
+				}
+			}
+			node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
+			node.m_q = node.m_x;
+			node.m_vn = node.m_v;
+		}
+		// enforce anchor constraints
+		for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
+		{
+			btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
+			btSoftBody::Node* n = a.m_node;
+			n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
+
+			// update multibody anchor info
+			if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+			{
+				btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
+				if (multibodyLinkCol)
+				{
+					btVector3 nrm;
+					const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
+					const btTransform& wtr = multibodyLinkCol->getWorldTransform();
+					psb->m_worldInfo->m_sparsesdf.Evaluate(
+						wtr.invXform(n->m_x),
+						shp,
+						nrm,
+						0);
+					a.m_cti.m_normal = wtr.getBasis() * nrm;
+					btVector3 normal = a.m_cti.m_normal;
+					btVector3 t1 = generateUnitOrthogonalVector(normal);
+					btVector3 t2 = btCross(normal, t1);
+					btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+					findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
+					findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
+					findJacobian(multibodyLinkCol, jacobianData_t2, a.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();
+					a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
+					a.jacobianData_normal = jacobianData_normal;
+					a.jacobianData_t1 = jacobianData_t1;
+					a.jacobianData_t2 = jacobianData_t2;
+					a.t1 = t1;
+					a.t2 = t2;
+				}
+			}
+		}
+		psb->interpolateRenderMesh();
+	}
+}

thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h

@@ -24,8 +24,8 @@
 #include "btConjugateResidual.h"
 #include "btConjugateGradient.h"
 struct btCollisionObjectWrapper;
-class btDeformableBackwardEulerObjective;
+// class btDeformableBackwardEulerObjective;
-class btDeformableMultiBodyDynamicsWorld;
+// class btDeformableMultiBodyDynamicsWorld;
 
 class btDeformableBodySolver : public btSoftBodySolver
 {
@@ -46,6 +46,7 @@ protected:
 	int m_maxNewtonIterations;                                     // max number of newton iterations
 	btScalar m_newtonTolerance;                                    // stop newton iterations if f(x) < m_newtonTolerance
 	bool m_lineSearch;                                             // If true, use newton's method with line search under implicit scheme
+	bool m_reducedSolver;																					 // flag for reduced soft body solver
 public:
 	// handles data related to objective function
 	btDeformableBackwardEulerObjective* m_objective;
@@ -68,11 +69,18 @@ public:
 	// solve the momentum equation
 	virtual void solveDeformableConstraints(btScalar solverdt);
 
+	// set gravity (get from deformable world)
+	virtual void setGravity(const btVector3& gravity)
+	{
+		// for full deformable object, we don't store gravity in the solver
+		// this function is overriden in the reduced deformable object
+	}
+
 	// resize/clear data structures
-	void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
+	virtual void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
 
 	// set up contact constraints
-	void setConstraints(const btContactSolverInfo& infoGlobal);
+	virtual void setConstraints(const btContactSolverInfo& infoGlobal);
 
 	// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
 	virtual void predictMotion(btScalar solverdt);
@@ -85,7 +93,7 @@ public:
 	void backupVelocity();
 
 	// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
-	void setupDeformableSolve(bool implicit);
+	virtual void setupDeformableSolve(bool implicit);
 
 	// set the current velocity to that backed up in m_backupVelocity
 	void revertVelocity();
@@ -150,6 +158,62 @@ public:
 	// used in line search
 	btScalar kineticEnergy();
 
+	// add explicit force to the velocity in the objective class
+	virtual void applyExplicitForce();
+
+	// execute position/velocity update and apply anchor constraints in the integrateTransforms from the Dynamics world
+	virtual void applyTransforms(btScalar timeStep);
+
+	virtual void setStrainLimiting(bool opt)
+	{
+		m_objective->m_projection.m_useStrainLimiting = opt;
+	}
+
+	virtual void setPreconditioner(int opt)
+	{
+		switch (opt)
+		{
+			case btDeformableBackwardEulerObjective::Mass_preconditioner:
+				m_objective->m_preconditioner = m_objective->m_massPreconditioner;
+				break;
+
+			case btDeformableBackwardEulerObjective::KKT_preconditioner:
+				m_objective->m_preconditioner = m_objective->m_KKTPreconditioner;
+				break;
+			
+			default:
+				btAssert(false);
+				break;
+		}
+	}
+
+	virtual btAlignedObjectArray<btDeformableLagrangianForce*>* getLagrangianForceArray()
+	{
+		return &(m_objective->m_lf);
+	}
+
+	virtual const btAlignedObjectArray<btSoftBody::Node*>* getIndices()
+	{
+		return m_objective->getIndices();
+	}
+
+	virtual void setProjection()
+	{
+		m_objective->m_projection.setProjection();
+	}
+
+	virtual void setLagrangeMultiplier()
+	{
+		m_objective->m_projection.setLagrangeMultiplier();
+	}
+
+	virtual bool isReducedSolver()
+	{
+		return m_reducedSolver;
+	}
+	
+	virtual void deformableBodyInternalWriteBack() {}
+
 	// unused functions
 	virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
 	virtual void solveConstraints(btScalar dt) {}

thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -268,7 +268,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 	{
 		dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
 	}
-	// dn is the normal component of velocity difference. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
 	btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
 	if (!infoGlobal.m_splitImpulse)
 	{
@@ -487,9 +487,6 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
 	btVector3 dv = impulse * contact->m_c2;
 	btSoftBody::Face* face = contact->m_face;
 
-	// save applied impulse
-	contact->m_cti.m_impulse = impulse;
-
 	btVector3& v0 = face->m_n[0]->m_v;
 	btVector3& v1 = face->m_n[1]->m_v;
 	btVector3& v2 = face->m_n[2]->m_v;

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -14,11 +14,16 @@
  */
 
 #include "btDeformableMultiBodyConstraintSolver.h"
+#include "BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
 #include <iostream>
+
 // override the iterations method to include deformable/multibody contact
 btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	{
+		// pair deformable body with solver body
+		pairDeformableAndSolverBody(bodies, numBodies, numDeformableBodies, infoGlobal);
+
 		///this is a special step to resolve penetrations (just for contacts)
 		solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
@@ -37,6 +42,10 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
 			// solver body velocity <- rigid body velocity
 			writeToSolverBody(bodies, numBodies, infoGlobal);
 
+
+			// std::cout << "------------Iteration " << iteration << "------------\n";
+			// std::cout << "m_leastSquaresResidual: " << m_leastSquaresResidual << "\n";
+
 			if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
 			{
 #ifdef VERBOSE_RESIDUAL_PRINTF
@@ -51,6 +60,9 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
 				m_analyticsData.m_numBodies = numBodies;
 				m_analyticsData.m_numContactManifolds = numManifolds;
 				m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
+				
+				m_deformableSolver->deformableBodyInternalWriteBack();
+				// std::cout << "[===================Next Step===================]\n";
 				break;
 			}
 		}
@@ -78,6 +90,12 @@ void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollision
 
 void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
+	// reduced soft body solver directly modifies the solver body
+	if (m_deformableSolver->isReducedSolver())
+	{
+		return;
+	}
+
 	for (int i = 0; i < numBodies; i++)
 	{
 		int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
@@ -94,6 +112,12 @@ void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject*
 
 void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
 {
+	// reduced soft body solver directly modifies the solver body
+	if (m_deformableSolver->isReducedSolver())
+	{
+		return;
+	}
+
 	for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
 	{
 		btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
@@ -105,6 +129,53 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
 	}
 }
 
+
+void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+{
+	if (!m_deformableSolver->isReducedSolver())
+	{
+		return;
+	}
+
+	btReducedDeformableBodySolver* solver = static_cast<btReducedDeformableBodySolver*>(m_deformableSolver);
+	
+	for (int i = 0; i < numDeformableBodies; ++i)
+	{
+		for (int k = 0; k < solver->m_nodeRigidConstraints[i].size(); ++k)
+    {
+      btReducedDeformableNodeRigidContactConstraint& constraint = solver->m_nodeRigidConstraints[i][k];
+
+			if (!constraint.m_contact->m_cti.m_colObj->isStaticObject())
+			{
+				btCollisionObject& col_obj = const_cast<btCollisionObject&>(*constraint.m_contact->m_cti.m_colObj);
+
+				// object index in the solver body pool
+				int bodyId = getOrInitSolverBody(col_obj, infoGlobal.m_timeStep);
+				
+				const btRigidBody* body = btRigidBody::upcast(bodies[bodyId]);
+				if (body && body->getInvMass())
+				{
+						// std::cout << "Node: " << constraint.m_node->index << ", body: " << bodyId << "\n";
+					btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
+					constraint.setSolverBody(bodyId, solverBody);
+				}
+			}
+    }
+
+		// for (int j = 0; j < numBodies; j++)
+		// {
+		// 	int bodyId = getOrInitSolverBody(*bodies[j], infoGlobal.m_timeStep);
+
+		// 	btRigidBody* body = btRigidBody::upcast(bodies[j]);
+		// 	if (body && body->getInvMass())
+		// 	{
+		// 		btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
+		// 		m_deformableSolver->pairConstraintWithSolverBody(i, bodyId, solverBody);
+		// 	}
+		// }	
+	}
+}
+
 void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h

@@ -43,6 +43,9 @@ protected:
 	// write the velocity of the underlying rigid body to the the the solver body
 	void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
+	// let each deformable body knows which solver body is in constact
+	void pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
 	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
 	virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -94,7 +94,7 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
 
 	beforeSolverCallbacks(timeStep);
 
-	///solve contact constraints and then deformable bodies momemtum equation
+	// ///solve contact constraints and then deformable bodies momemtum equation
 	solveConstraints(timeStep);
 
 	afterSolverCallbacks(timeStep);
@@ -201,7 +201,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 			if (psb->isActive())
 			{
 				// clear contact points in the previous iteration
-				psb->m_faceNodeContacts.clear();
+				psb->m_faceNodeContactsCCD.clear();
 
 				// update m_q and normals for CCD calculation
 				for (int j = 0; j < psb->m_nodes.size(); ++j)
@@ -237,7 +237,8 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 			btSoftBody* psb = m_softBodies[i];
 			if (psb->isActive())
 			{
-				penetration_count += psb->m_faceNodeContacts.size();
+				penetration_count += psb->m_faceNodeContactsCCD.size();
+				;
 			}
 		}
 		if (penetration_count == 0)
@@ -297,83 +298,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 	BT_PROFILE("integrateTransforms");
 	positionCorrection(timeStep);
 	btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
-	for (int i = 0; i < m_softBodies.size(); ++i)
+	m_deformableBodySolver->applyTransforms(timeStep);
-	{
-		btSoftBody* psb = m_softBodies[i];
-		for (int j = 0; j < psb->m_nodes.size(); ++j)
-		{
-			btSoftBody::Node& node = psb->m_nodes[j];
-			btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
-			btScalar clampDeltaV = maxDisplacement / timeStep;
-			for (int c = 0; c < 3; c++)
-			{
-				if (node.m_v[c] > clampDeltaV)
-				{
-					node.m_v[c] = clampDeltaV;
-				}
-				if (node.m_v[c] < -clampDeltaV)
-				{
-					node.m_v[c] = -clampDeltaV;
-				}
-			}
-			node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
-			node.m_q = node.m_x;
-			node.m_vn = node.m_v;
-		}
-		// enforce anchor constraints
-		for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
-		{
-			btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
-			btSoftBody::Node* n = a.m_node;
-			n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
-
-			// update multibody anchor info
-			if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-			{
-				btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
-				if (multibodyLinkCol)
-				{
-					btVector3 nrm;
-					const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
-					const btTransform& wtr = multibodyLinkCol->getWorldTransform();
-					psb->m_worldInfo->m_sparsesdf.Evaluate(
-						wtr.invXform(n->m_x),
-						shp,
-						nrm,
-						0);
-					a.m_cti.m_normal = wtr.getBasis() * nrm;
-					btVector3 normal = a.m_cti.m_normal;
-					btVector3 t1 = generateUnitOrthogonalVector(normal);
-					btVector3 t2 = btCross(normal, t1);
-					btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
-					findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
-					findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
-					findJacobian(multibodyLinkCol, jacobianData_t2, a.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();
-					a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
-					a.jacobianData_normal = jacobianData_normal;
-					a.jacobianData_t1 = jacobianData_t1;
-					a.jacobianData_t2 = jacobianData_t2;
-					a.t1 = t1;
-					a.t2 = t2;
-				}
-			}
-		}
-		psb->interpolateRenderMesh();
-	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
@@ -390,9 +315,9 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 
 	// set up the directions in which the velocity does not change in the momentum solve
 	if (m_useProjection)
-		m_deformableBodySolver->m_objective->m_projection.setProjection();
+		m_deformableBodySolver->setProjection();
 	else
-		m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier();
+		m_deformableBodySolver->setLagrangeMultiplier();
 
 	// for explicit scheme, m_backupVelocity = v_{n+1}^*
 	// for implicit scheme, m_backupVelocity = v_n
@@ -518,6 +443,12 @@ void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar time
 	m_deformableBodySolver->predictMotion(timeStep);
 }
 
+void btDeformableMultiBodyDynamicsWorld::setGravity(const btVector3& gravity)
+{
+	btDiscreteDynamicsWorld::setGravity(gravity);
+	m_deformableBodySolver->setGravity(gravity);
+}
+
 void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 {
 	m_internalTime += timeStep;
@@ -532,14 +463,14 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 	if (m_useProjection)
 	{
 		m_deformableBodySolver->m_useProjection = true;
-		m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
+		m_deformableBodySolver->setStrainLimiting(true);
-		m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner;
+		m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::Mass_preconditioner);
 	}
 	else
 	{
 		m_deformableBodySolver->m_useProjection = false;
-		m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
+		m_deformableBodySolver->setStrainLimiting(false);
-		m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
+		m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::KKT_preconditioner);
 	}
 }
 
@@ -681,7 +612,7 @@ void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
 
 void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
 {
-	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
 	bool added = false;
 	for (int i = 0; i < forces.size(); ++i)
 	{
@@ -695,14 +626,14 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL
 	if (!added)
 	{
 		force->addSoftBody(psb);
-		force->setIndices(m_deformableBodySolver->m_objective->getIndices());
+		force->setIndices(m_deformableBodySolver->getIndices());
 		forces.push_back(force);
 	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force)
 {
-	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
 	int removed_index = -1;
 	for (int i = 0; i < forces.size(); ++i)
 	{
@@ -720,7 +651,7 @@ void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformab
 
 void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb)
 {
-	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
 	for (int i = 0; i < forces.size(); ++i)
 	{
 		forces[i]->removeSoftBody(psb);

thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -19,7 +19,7 @@
 #include "btSoftMultiBodyDynamicsWorld.h"
 #include "btDeformableLagrangianForce.h"
 #include "btDeformableMassSpringForce.h"
-#include "btDeformableBodySolver.h"
+// #include "btDeformableBodySolver.h"
 #include "btDeformableMultiBodyConstraintSolver.h"
 #include "btSoftBodyHelpers.h"
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
@@ -121,6 +121,8 @@ public:
 		return m_sbi;
 	}
 
+	virtual void setGravity(const btVector3& gravity);
+
 	void reinitialize(btScalar timeStep);
 
 	void applyRigidBodyGravity(btScalar timeStep);

thirdparty/bullet/BulletSoftBody/btSoftBody.cpp

@@ -231,6 +231,9 @@ void btSoftBody::initDefaults()
 	m_gravityFactor = 1;
 	m_cacheBarycenter = false;
 	m_fdbvnt = 0;
+
+	// reduced flag
+	m_reducedModel = false;
 }
 
 //
@@ -1483,6 +1486,21 @@ void btSoftBody::randomizeConstraints()
 #undef NEXTRAND
 }
 
+void btSoftBody::updateState(const btAlignedObjectArray<btVector3>& q, const btAlignedObjectArray<btVector3>& v)
+{
+	int node_count = m_nodes.size();
+	btAssert(node_count == q.size());
+	btAssert(node_count == v.size());
+	for (int i = 0; i < node_count; i++)
+	{
+		Node& n = m_nodes[i];
+		n.m_x = q[i];
+		n.m_q = q[i];
+		n.m_v = v[i];
+		n.m_vn = v[i];
+	}
+}
+
 //
 void btSoftBody::releaseCluster(int index)
 {
@@ -2821,7 +2839,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 	btScalar dst;
 	btGjkEpaSolver2::sResults results;
 
-//	#define USE_QUADRATURE 1
+	//	#define USE_QUADRATURE 1
 
 	// use collision quadrature point
 #ifdef USE_QUADRATURE
@@ -3879,7 +3897,7 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
 			btVector3 va(0, 0, 0);
 			btRigidBody* rigidCol = 0;
 			btMultiBodyLinkCollider* multibodyLinkCol = 0;
-			btScalar* deltaV;
+			btScalar* deltaV = NULL;
 
 			if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 			{

thirdparty/bullet/BulletSoftBody/btSoftBody.h

@@ -223,12 +223,10 @@ public:
 	/* sCti is Softbody contact info	*/
 	struct sCti
 	{
-		const btCollisionObject* m_colObj; /* Rigid body			        */
+		const btCollisionObject* m_colObj; /* Rigid body			*/
-		btVector3 m_normal;                /* Outward normal		        */
+		btVector3 m_normal;                /* Outward normal		*/
-		mutable btVector3 m_impulse;	   /* Applied impulse        	    */
+		btScalar m_offset;                 /* Offset from origin	*/
-		btScalar m_offset;                 /* Offset from origin	        */
 		btVector3 m_bary;                  /* Barycentric weights for faces */
-		sCti() : m_impulse(0, 0, 0) {}
 	};
 
 	/* sMedium		*/
@@ -300,7 +298,7 @@ public:
 	};
 	struct RenderFace
 	{
-		RenderNode* m_n[3];          // Node pointers
+		RenderNode* m_n[3];  // Node pointers
 	};
 
 	/* Face			*/
@@ -409,6 +407,7 @@ public:
 		btScalar m_friction;  // Friction
 		btScalar m_imf;       // inverse mass of the face at contact point
 		btScalar m_c0;        // scale of the impulse matrix;
+		const btCollisionObject* m_colObj;  // Collision object to collide with.
 	};
 
 	/* SContact		*/
@@ -788,7 +787,7 @@ public:
 	typedef btAlignedObjectArray<Cluster*> tClusterArray;
 	typedef btAlignedObjectArray<Note> tNoteArray;
 	typedef btAlignedObjectArray<Node> tNodeArray;
-	typedef btAlignedObjectArray< RenderNode> tRenderNodeArray;
+	typedef btAlignedObjectArray<RenderNode> tRenderNodeArray;
 	typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
 	typedef btAlignedObjectArray<Link> tLinkArray;
 	typedef btAlignedObjectArray<Face> tFaceArray;
@@ -800,6 +799,7 @@ public:
 	typedef btAlignedObjectArray<Material*> tMaterialArray;
 	typedef btAlignedObjectArray<Joint*> tJointArray;
 	typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
+	typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
 
 	//
 	// Fields
@@ -815,7 +815,7 @@ public:
 	tRenderNodeArray m_renderNodes;    // Render Nodes
 	tLinkArray m_links;                // Links
 	tFaceArray m_faces;                // Faces
-	tRenderFaceArray m_renderFaces;          // Faces
+	tRenderFaceArray m_renderFaces;    // Faces
 	tTetraArray m_tetras;              // Tetras
 	btAlignedObjectArray<TetraScratch> m_tetraScratches;
 	btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
@@ -825,6 +825,7 @@ public:
 	btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
 	btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
 	btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
+	btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContactsCCD;
 	tSContactArray m_scontacts;     // Soft contacts
 	tJointArray m_joints;           // Joints
 	tMaterialArray m_materials;     // Materials
@@ -857,6 +858,8 @@ public:
 
 	btScalar m_restLengthScale;
 
+	bool m_reducedModel;	// Reduced deformable model flag
+	
 	//
 	// Api
 	//
@@ -894,7 +897,7 @@ public:
 				   int node1) const;
 	bool checkLink(const Node* node0,
 				   const Node* node1) const;
-	/* Check for existing face												*/
+	/* Check for existring face												*/
 	bool checkFace(int node0,
 				   int node1,
 				   int node2) const;
@@ -1003,15 +1006,15 @@ public:
 	/* Get best fit rigid transform                                         */
 	btTransform getRigidTransform();
 	/* Transform to given pose                                              */
-	void transformTo(const btTransform& trs);
+	virtual void transformTo(const btTransform& trs);
 	/* Transform															*/
-	void transform(const btTransform& trs);
+	virtual void transform(const btTransform& trs);
 	/* Translate															*/
-	void translate(const btVector3& trs);
+	virtual void translate(const btVector3& trs);
 	/* Rotate															*/
-	void rotate(const btQuaternion& rot);
+	virtual void rotate(const btQuaternion& rot);
 	/* Scale																*/
-	void scale(const btVector3& scl);
+	virtual void scale(const btVector3& scl);
 	/* Get link resting lengths scale										*/
 	btScalar getRestLengthScale();
 	/* Scale resting length of all springs									*/
@@ -1053,6 +1056,9 @@ public:
 								   Material* mat = 0);
 	/* Randomize constraints to reduce solver bias							*/
 	void randomizeConstraints();
+
+	void updateState(const btAlignedObjectArray<btVector3>& qs, const btAlignedObjectArray<btVector3>& vs);
+
 	/* Release clusters														*/
 	void releaseCluster(int index);
 	void releaseClusters();
@@ -1098,6 +1104,13 @@ public:
 	void setZeroVelocity();
 	bool wantsSleeping();
 
+	virtual btMatrix3x3 getImpulseFactor(int n_node)
+	{
+		btMatrix3x3 tmp;
+		tmp.setIdentity();
+		return tmp;
+	}
+
 	//
 	// Functionality to deal with new accelerated solvers.
 	//

thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -18,6 +18,7 @@ subject to the following restrictions:
 #include <stdio.h>
 #include <string>
 #include <iostream>
+#include <iomanip> 
 #include <sstream>
 #include <string.h>
 #include <algorithm>
@@ -1487,6 +1488,37 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
 	fs.close();
 }
 
+
+void btSoftBodyHelpers::writeState(const char* file, const btSoftBody* psb)
+{
+	std::ofstream fs;
+	fs.open(file);
+	btAssert(fs);
+	fs << std::scientific << std::setprecision(16);
+
+	// Only write out for trimesh, directly write out all the nodes and faces.xs
+	for (int i = 0; i < psb->m_nodes.size(); ++i)
+	{
+		fs << "q";
+		for (int d = 0; d < 3; d++)
+		{
+			fs << " " << psb->m_nodes[i].m_q[d];
+		}
+		fs << "\n";
+	}
+
+	for (int i = 0; i < psb->m_nodes.size(); ++i)
+	{
+		fs << "v";
+		for (int d = 0; d < 3; d++)
+		{
+			fs << " " << psb->m_nodes[i].m_v[d];
+		}
+		fs << "\n";
+	}
+	fs.close();
+}
+
 void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
 {
 	std::ifstream fs_read;

thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h

@@ -146,6 +146,11 @@ struct btSoftBodyHelpers
 
 	static void writeObj(const char* file, const btSoftBody* psb);
 
+	static void writeState(const char* file, const btSoftBody* psb);
+
+  //this code cannot be here, dependency on example code are not allowed
+	//static std::string loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO);
+
 	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
 
 	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary);

thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h

@@ -1691,12 +1691,19 @@ struct btSoftColliders
 						if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 						{
 							const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
-							static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
-							const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
 							const btVector3 ra = n.m_x - wtr.getOrigin();
 
-							c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
+							static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
-							//                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+							const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+							if (psb->m_reducedModel)
+							{
+								c.m_c0 = MassMatrix(imb, iwi, ra); //impulse factor K of the rigid body only (not the inverse)
+							}
+							else
+							{
+								c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
+								//                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+							}
 							c.m_c1 = ra;
 						}
 						else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
@@ -1724,7 +1731,16 @@ struct btSoftColliders
 												t1.getX(), t1.getY(), t1.getZ(),
 												t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
 								const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-								btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+								
+								btMatrix3x3 local_impulse_matrix;
+								if (psb->m_reducedModel)
+								{
+									local_impulse_matrix = OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof);
+								}
+								else
+								{
+									local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+								}
 								c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
 								c.jacobianData_normal = jacobianData_normal;
 								c.jacobianData_t1 = jacobianData_t1;
@@ -1944,6 +1960,7 @@ struct btSoftColliders
 					c.m_weights = btVector3(0, 0, 0);
 					c.m_imf = 0;
 					c.m_c0 = 0;
+					c.m_colObj = psb[1];
 					psb[0]->m_faceNodeContacts.push_back(c);
 				}
 			}
@@ -2019,6 +2036,7 @@ struct btSoftColliders
 				c.m_weights = btVector3(0, 0, 0);
 				c.m_imf = 0;
 				c.m_c0 = 0;
+				c.m_colObj = psb[1];
 				psb[0]->m_faceNodeContacts.push_back(c);
 			}
 		}
@@ -2050,7 +2068,8 @@ struct btSoftColliders
 				c.m_margin = mrg;
 				c.m_imf = 0;
 				c.m_c0 = 0;
-				psb[0]->m_faceNodeContacts.push_back(c);
+				c.m_colObj = psb[1];
+				psb[0]->m_faceNodeContactsCCD.push_back(c);
 			}
 		}
 		void Process(const btDbvntNode* lface1,
@@ -2114,7 +2133,8 @@ struct btSoftColliders
 					c.m_margin = mrg;
 					c.m_imf = 0;
 					c.m_c0 = 0;
-					psb[0]->m_faceNodeContacts.push_back(c);
+					c.m_colObj = psb[1];
+					psb[0]->m_faceNodeContactsCCD.push_back(c);
 				}
 			}
 		}

thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h

@@ -36,7 +36,8 @@ public:
 		CL_SIMD_SOLVER,
 		DX_SOLVER,
 		DX_SIMD_SOLVER,
-		DEFORMABLE_SOLVER
+		DEFORMABLE_SOLVER,
+		REDUCED_DEFORMABLE_SOLVER
 	};
 
 protected:

thirdparty/bullet/LinearMath/btScalar.h

@@ -25,7 +25,7 @@ subject to the following restrictions:
 #include <float.h>
 
 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
-#define BT_BULLET_VERSION 320
+#define BT_BULLET_VERSION 324
 
 inline int btGetVersion()
 {
@@ -107,7 +107,7 @@ inline int btIsDoublePrecision()
  			#define btFsel(a,b,c) __fsel((a),(b),(c))
 		#else
 
-#if defined (_M_ARM)
+#if defined (_M_ARM) || defined (_M_ARM64)
             //Do not turn SSE on for ARM (may want to turn on BT_USE_NEON however)
 #elif (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
 

thirdparty/bullet/LinearMath/btSerializer.h

@@ -481,7 +481,7 @@ public:
 
 		buffer[9] = '3';
 		buffer[10] = '2';
-		buffer[11] = '1';
+		buffer[11] = '4';
 	}
 
 	virtual void startSerialization()

thirdparty/bullet/LinearMath/btTransform.h

@@ -34,6 +34,7 @@ btTransform
 	btVector3 m_origin;
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 	/**@brief No initialization constructor */
 	btTransform() {}
 	/**@brief Constructor from btQuaternion (optional btVector3 )

thirdparty/bullet/VERSION.txt

@@ -1 +1 @@
-3.21
+3.24

thirdparty/bullet/btBulletDynamicsAll.cpp

@@ -36,6 +36,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"
 #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"
 #include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp"
 #include "BulletDynamics/Vehicle/btRaycastVehicle.cpp"
 #include "BulletDynamics/Vehicle/btWheelInfo.cpp"
 #include "BulletDynamics/Character/btKinematicCharacterController.cpp"

thirdparty/bullet/patches/fix-reorder-warning.patch

@@ -1,38 +0,0 @@
-diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
-index f2cc3409f9..70915366e0 100644
---- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
-+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
-@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
- 
- 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, 
-+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.
-@@ -71,9 +71,9 @@ public:
- 					const btVector3& normal,
- 					btScalar distance) : m_localPointA(pointA),
- 										 m_localPointB(pointB),
-+										 m_positionWorldOnB(0,0,0),
-+										 m_positionWorldOnA(0,0,0),
- 										 m_normalWorldOnB(normal),
--                     m_positionWorldOnB(0,0,0),
--                     m_positionWorldOnA(0,0,0),
- 										 m_distance1(distance),
- 										 m_combinedFriction(btScalar(0.)),
- 										 m_combinedRollingFriction(btScalar(0.)),
-@@ -95,8 +95,8 @@ public:
- 										 m_contactERP(0.f),
- 										 m_frictionCFM(0.f),
- 										 m_lifeTime(0),
--            m_lateralFrictionDir1(0,0,0),
--            m_lateralFrictionDir2(0,0,0)
-+										 m_lateralFrictionDir1(0,0,0),
-+										 m_lateralFrictionDir2(0,0,0)
- 	{
- 	}
-