Merge branch 'master' of https://github.com/erwincoumans/bullet3

Extras/Serialize/BulletFileLoader/autogenerated/bullet.h

@@ -19,11 +19,10 @@
 // Auto generated from Bullet/Extras/HeaderGenerator/bulletGenerate.py
 #ifndef __BULLET_H__
 #define __BULLET_H__
-namespace Bullet
+namespace Bullet {
-{
+
 // put an empty struct in the case
-typedef struct bInvalidHandle
+typedef struct bInvalidHandle {
-{
 	int unused;
 }bInvalidHandle;
 
@@ -124,6 +123,7 @@ public:
         void *m_data;
     };
 
+
 // -------------------------------------------------- //
     class btPhysicsSystem
     {
@@ -133,6 +133,7 @@ public:
         PointerArray m_constraints;
     };
 
+
 // -------------------------------------------------- //
     class ListBase
     {
@@ -141,6 +142,7 @@ public:
         void *last;
     };
 
+
 // -------------------------------------------------- //
     class btVector3FloatData
     {
@@ -148,6 +150,7 @@ public:
         float m_floats[4];
     };
 
+
 // -------------------------------------------------- //
     class btVector3DoubleData
     {
@@ -155,6 +158,7 @@ public:
         double m_floats[4];
     };
 
+
 // -------------------------------------------------- //
     class btQuaternionFloatData
     {
@@ -162,6 +166,7 @@ public:
         float m_floats[4];
     };
 
+
 // -------------------------------------------------- //
     class btQuaternionDoubleData
     {
@@ -169,6 +174,7 @@ public:
         double m_floats[4];
     };
 
+
 // -------------------------------------------------- //
     class btMatrix3x3FloatData
     {
@@ -176,6 +182,7 @@ public:
         btVector3FloatData m_el[3];
     };
 
+
 // -------------------------------------------------- //
     class btMatrix3x3DoubleData
     {
@@ -183,6 +190,7 @@ public:
         btVector3DoubleData m_el[3];
     };
 
+
 // -------------------------------------------------- //
     class btTransformFloatData
     {
@@ -191,6 +199,7 @@ public:
         btVector3FloatData m_origin;
     };
 
+
 // -------------------------------------------------- //
     class btTransformDoubleData
     {
@@ -199,6 +208,7 @@ public:
         btVector3DoubleData m_origin;
     };
 
+
 // -------------------------------------------------- //
     class btBvhSubtreeInfoData
     {
@@ -209,6 +219,7 @@ public:
         short m_quantizedAabbMax[3];
     };
 
+
 // -------------------------------------------------- //
     class btOptimizedBvhNodeFloatData
     {
@@ -221,6 +232,7 @@ public:
         char m_pad[4];
     };
 
+
 // -------------------------------------------------- //
     class btOptimizedBvhNodeDoubleData
     {
@@ -233,6 +245,7 @@ public:
         char m_pad[4];
     };
 
+
 // -------------------------------------------------- //
     class btQuantizedBvhNodeData
     {
@@ -242,6 +255,7 @@ public:
         int m_escapeIndexOrTriangleIndex;
     };
 
+
 // -------------------------------------------------- //
     class btQuantizedBvhFloatData
     {
@@ -260,6 +274,7 @@ public:
         int m_numSubtreeHeaders;
     };
 
+
 // -------------------------------------------------- //
     class btQuantizedBvhDoubleData
     {
@@ -278,6 +293,7 @@ public:
         btBvhSubtreeInfoData *m_subTreeInfoPtr;
     };
 
+
 // -------------------------------------------------- //
     class btCollisionShapeData
     {
@@ -287,6 +303,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btStaticPlaneShapeData
     {
@@ -298,6 +315,7 @@ public:
         char m_pad[4];
     };
 
+
 // -------------------------------------------------- //
     class btConvexInternalShapeData
     {
@@ -309,6 +327,7 @@ public:
         int m_padding;
     };
 
+
 // -------------------------------------------------- //
     class btPositionAndRadius
     {
@@ -317,6 +336,7 @@ public:
         float m_radius;
     };
 
+
 // -------------------------------------------------- //
     class btMultiSphereShapeData
     {
@@ -327,6 +347,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btIntIndexData
     {
@@ -334,6 +355,7 @@ public:
         int m_value;
     };
 
+
 // -------------------------------------------------- //
     class btShortIntIndexData
     {
@@ -342,6 +364,7 @@ public:
         char m_pad[2];
     };
 
+
 // -------------------------------------------------- //
     class btShortIntIndexTripletData
     {
@@ -350,6 +373,7 @@ public:
         char m_pad[2];
     };
 
+
 // -------------------------------------------------- //
     class btCharIndexTripletData
     {
@@ -358,6 +382,7 @@ public:
         char m_pad;
     };
 
+
 // -------------------------------------------------- //
     class btMeshPartData
     {
@@ -372,6 +397,7 @@ public:
         int m_numVertices;
     };
 
+
 // -------------------------------------------------- //
     class btStridingMeshInterfaceData
     {
@@ -382,6 +408,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btTriangleMeshShapeData
     {
@@ -395,6 +422,7 @@ public:
         char m_pad3[4];
     };
 
+
 // -------------------------------------------------- //
     class btScaledTriangleMeshShapeData
     {
@@ -403,6 +431,7 @@ public:
         btVector3FloatData m_localScaling;
     };
 
+
 // -------------------------------------------------- //
     class btCompoundShapeChildData
     {
@@ -413,6 +442,7 @@ public:
         float m_childMargin;
     };
 
+
 // -------------------------------------------------- //
     class btCompoundShapeData
     {
@@ -423,6 +453,7 @@ public:
         float m_collisionMargin;
     };
 
+
 // -------------------------------------------------- //
     class btCylinderShapeData
     {
@@ -432,6 +463,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btConeShapeData
     {
@@ -441,6 +473,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btCapsuleShapeData
     {
@@ -450,6 +483,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btTriangleInfoData
     {
@@ -460,6 +494,7 @@ public:
         float m_edgeV2V0Angle;
     };
 
+
 // -------------------------------------------------- //
     class btTriangleInfoMapData
     {
@@ -480,6 +515,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btPersistentManifoldDoubleData
     {
@@ -493,6 +529,7 @@ public:
         btVector3DoubleData m_pointCacheLateralFrictionDir2[4];
         double m_pointCacheDistance[4];
         double m_pointCacheAppliedImpulse[4];
+        double m_pointCachePrevRHS[4];
         double m_pointCacheCombinedFriction[4];
         double m_pointCacheCombinedRollingFriction[4];
         double m_pointCacheCombinedSpinningFriction[4];
@@ -524,6 +561,7 @@ public:
         btCollisionObjectDoubleData *m_body1;
     };
 
+
 // -------------------------------------------------- //
     class btPersistentManifoldFloatData
     {
@@ -537,6 +575,7 @@ public:
         btVector3FloatData m_pointCacheLateralFrictionDir2[4];
         float m_pointCacheDistance[4];
         float m_pointCacheAppliedImpulse[4];
+        float m_pointCachePrevRHS[4];
         float m_pointCacheCombinedFriction[4];
         float m_pointCacheCombinedRollingFriction[4];
         float m_pointCacheCombinedSpinningFriction[4];
@@ -568,6 +607,7 @@ public:
         btCollisionObjectFloatData *m_body1;
     };
 
+
 // -------------------------------------------------- //
     class btGImpactMeshShapeData
     {
@@ -579,6 +619,7 @@ public:
         int m_gimpactSubType;
     };
 
+
 // -------------------------------------------------- //
     class btConvexHullShapeData
     {
@@ -590,6 +631,7 @@ public:
         char m_padding3[4];
     };
 
+
 // -------------------------------------------------- //
     class btCollisionObjectDoubleData
     {
@@ -625,6 +667,7 @@ public:
         int m_uniqueId;
     };
 
+
 // -------------------------------------------------- //
     class btCollisionObjectFloatData
     {
@@ -660,6 +703,7 @@ public:
         int m_uniqueId;
     };
 
+
 // -------------------------------------------------- //
     class btContactSolverInfoDoubleData
     {
@@ -678,6 +722,7 @@ public:
         double m_splitImpulseTurnErp;
         double m_linearSlop;
         double m_warmstartingFactor;
+        double m_articulatedWarmstartingFactor;
         double m_maxGyroscopicForce;
         double m_singleAxisRollingFrictionThreshold;
         int m_numIterations;
@@ -688,6 +733,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btContactSolverInfoFloatData
     {
@@ -706,6 +752,7 @@ public:
         float m_splitImpulseTurnErp;
         float m_linearSlop;
         float m_warmstartingFactor;
+        float m_articulatedWarmstartingFactor;
         float m_maxGyroscopicForce;
         float m_singleAxisRollingFrictionThreshold;
         int m_numIterations;
@@ -716,6 +763,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btDynamicsWorldDoubleData
     {
@@ -724,6 +772,7 @@ public:
         btVector3DoubleData m_gravity;
     };
 
+
 // -------------------------------------------------- //
     class btDynamicsWorldFloatData
     {
@@ -732,6 +781,7 @@ public:
         btVector3FloatData m_gravity;
     };
 
+
 // -------------------------------------------------- //
     class btRigidBodyFloatData
     {
@@ -759,6 +809,7 @@ public:
         int m_additionalDamping;
     };
 
+
 // -------------------------------------------------- //
     class btRigidBodyDoubleData
     {
@@ -787,6 +838,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btConstraintInfo1
     {
@@ -795,6 +847,7 @@ public:
         int nub;
     };
 
+
 // -------------------------------------------------- //
     class btTypedConstraintFloatData
     {
@@ -814,6 +867,7 @@ public:
         int m_isEnabled;
     };
 
+
 // -------------------------------------------------- //
     class btTypedConstraintData
     {
@@ -833,6 +887,7 @@ public:
         int m_isEnabled;
     };
 
+
 // -------------------------------------------------- //
     class btTypedConstraintDoubleData
     {
@@ -853,6 +908,7 @@ public:
         char padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btPoint2PointConstraintFloatData
     {
@@ -862,6 +918,7 @@ public:
         btVector3FloatData m_pivotInB;
     };
 
+
 // -------------------------------------------------- //
     class btPoint2PointConstraintDoubleData2
     {
@@ -871,6 +928,7 @@ public:
         btVector3DoubleData m_pivotInB;
     };
 
+
 // -------------------------------------------------- //
     class btPoint2PointConstraintDoubleData
     {
@@ -880,6 +938,7 @@ public:
         btVector3DoubleData m_pivotInB;
     };
 
+
 // -------------------------------------------------- //
     class btHingeConstraintDoubleData
     {
@@ -899,6 +958,7 @@ public:
         float m_relaxationFactor;
     };
 
+
 // -------------------------------------------------- //
     class btHingeConstraintFloatData
     {
@@ -918,6 +978,7 @@ public:
         float m_relaxationFactor;
     };
 
+
 // -------------------------------------------------- //
     class btHingeConstraintDoubleData2
     {
@@ -938,6 +999,7 @@ public:
         char m_padding1[4];
     };
 
+
 // -------------------------------------------------- //
     class btConeTwistConstraintDoubleData
     {
@@ -954,6 +1016,7 @@ public:
         double m_damping;
     };
 
+
 // -------------------------------------------------- //
     class btConeTwistConstraintData
     {
@@ -971,6 +1034,7 @@ public:
         char m_pad[4];
     };
 
+
 // -------------------------------------------------- //
     class btGeneric6DofConstraintData
     {
@@ -986,6 +1050,7 @@ public:
         int m_useOffsetForConstraintFrame;
     };
 
+
 // -------------------------------------------------- //
     class btGeneric6DofConstraintDoubleData2
     {
@@ -1001,6 +1066,7 @@ public:
         int m_useOffsetForConstraintFrame;
     };
 
+
 // -------------------------------------------------- //
     class btGeneric6DofSpringConstraintData
     {
@@ -1012,6 +1078,7 @@ public:
         float m_springDamping[6];
     };
 
+
 // -------------------------------------------------- //
     class btGeneric6DofSpringConstraintDoubleData2
     {
@@ -1023,6 +1090,7 @@ public:
         double m_springDamping[6];
     };
 
+
 // -------------------------------------------------- //
     class btGeneric6DofSpring2ConstraintData
     {
@@ -1070,6 +1138,7 @@ public:
         int m_rotateOrder;
     };
 
+
 // -------------------------------------------------- //
     class btGeneric6DofSpring2ConstraintDoubleData2
     {
@@ -1117,6 +1186,7 @@ public:
         int m_rotateOrder;
     };
 
+
 // -------------------------------------------------- //
     class btSliderConstraintData
     {
@@ -1132,6 +1202,7 @@ public:
         int m_useOffsetForConstraintFrame;
     };
 
+
 // -------------------------------------------------- //
     class btSliderConstraintDoubleData
     {
@@ -1147,6 +1218,7 @@ public:
         int m_useOffsetForConstraintFrame;
     };
 
+
 // -------------------------------------------------- //
     class btGearConstraintFloatData
     {
@@ -1158,6 +1230,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btGearConstraintDoubleData
     {
@@ -1168,6 +1241,7 @@ public:
         double m_ratio;
     };
 
+
 // -------------------------------------------------- //
     class SoftBodyMaterialData
     {
@@ -1178,6 +1252,7 @@ public:
         int m_flags;
     };
 
+
 // -------------------------------------------------- //
     class SoftBodyNodeData
     {
@@ -1194,6 +1269,7 @@ public:
         int m_pad;
     };
 
+
 // -------------------------------------------------- //
     class SoftBodyLinkData
     {
@@ -1204,6 +1280,7 @@ public:
         int m_bbending;
     };
 
+
 // -------------------------------------------------- //
     class SoftBodyFaceData
     {
@@ -1214,6 +1291,7 @@ public:
         float m_restArea;
     };
 
+
 // -------------------------------------------------- //
     class SoftBodyTetraData
     {
@@ -1227,6 +1305,7 @@ public:
         int m_pad;
     };
 
+
 // -------------------------------------------------- //
     class SoftRigidAnchorData
     {
@@ -1239,6 +1318,7 @@ public:
         float m_c2;
     };
 
+
 // -------------------------------------------------- //
     class SoftBodyConfigData
     {
@@ -1271,6 +1351,7 @@ public:
         int m_collisionFlags;
     };
 
+
 // -------------------------------------------------- //
     class SoftBodyPoseData
     {
@@ -1289,6 +1370,7 @@ public:
         int m_pad;
     };
 
+
 // -------------------------------------------------- //
     class SoftBodyClusterData
     {
@@ -1322,6 +1404,7 @@ public:
         int m_clusterIndex;
     };
 
+
 // -------------------------------------------------- //
     class btSoftBodyJointData
     {
@@ -1340,6 +1423,7 @@ public:
         int m_pad;
     };
 
+
 // -------------------------------------------------- //
     class btSoftBodyFloatData
     {
@@ -1365,6 +1449,7 @@ public:
         SoftBodyConfigData m_config;
     };
 
+
 // -------------------------------------------------- //
     class btMultiBodyLinkDoubleData
     {
@@ -1399,6 +1484,7 @@ public:
         char *m_paddingPtr;
     };
 
+
 // -------------------------------------------------- //
     class btMultiBodyLinkFloatData
     {
@@ -1433,6 +1519,7 @@ public:
         char *m_paddingPtr;
     };
 
+
 // -------------------------------------------------- //
     class btMultiBodyDoubleData
     {
@@ -1450,6 +1537,7 @@ public:
         btCollisionObjectDoubleData *m_baseCollider;
     };
 
+
 // -------------------------------------------------- //
     class btMultiBodyFloatData
     {
@@ -1466,6 +1554,7 @@ public:
         btCollisionObjectFloatData *m_baseCollider;
     };
 
+
 // -------------------------------------------------- //
     class btMultiBodyLinkColliderFloatData
     {
@@ -1476,6 +1565,7 @@ public:
         char m_padding[4];
     };
 
+
 // -------------------------------------------------- //
     class btMultiBodyLinkColliderDoubleData
     {
@@ -1486,5 +1576,6 @@ public:
         char m_padding[4];
     };
 
-}  // namespace Bullet
+
+}
 #endif//__BULLET_H__

README.md

@@ -44,6 +44,18 @@ track down the issue, but more work is required to cover all OpenCL kernels.
 All source code files are licensed under the permissive zlib license
 (http://opensource.org/licenses/Zlib) unless marked differently in a particular folder/file.
 
+## Build instructions for Bullet using vcpkg
+
+You can download and install Bullet using the [vcpkg](https://github.com/Microsoft/vcpkg/) dependency manager:
+
+    git clone https://github.com/Microsoft/vcpkg.git
+    cd vcpkg
+    ./bootstrap-vcpkg.sh
+    ./vcpkg integrate install
+    vcpkg install bullet3
+
+The Bullet port in vcpkg is kept up to date by Microsoft team members and community contributors. If the version is out of date, please [create an issue or pull request](https://github.com/Microsoft/vcpkg) on the vcpkg repository.
+
 ## Build instructions for Bullet using premake. You can also use cmake instead.
 
 **Windows**

VERSION

@@ -1 +1 @@
-2.88
+2.89

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -752,6 +752,14 @@ B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandl
 	return 0;
 }
 
+B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+	command->m_physSimParamArgs.m_articulatedWarmStartingFactor = warmStartingFactor;
+	command->m_updateFlags |= SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR;
+	return 0;
+}
 B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -339,6 +339,7 @@ extern "C"
 	B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
 	B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
 	B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
+	B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
 	B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
 	B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
 	B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -1458,7 +1458,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
                                BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
                                m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
                                bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
-                               bodyJoints->m_baseName = "baseLink";
+                               bodyJoints->m_baseName = serverCmd.m_dataStreamArguments.m_bodyName;
                                         
                                if (bf.ok())
                                {

examples/SharedMemory/PhysicsDirect.cpp

@@ -1218,7 +1218,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
                        BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
                        m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
                        bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
-                       bodyJoints->m_baseName = "baseLink";
+                       bodyJoints->m_baseName = serverCmd.m_dataStreamArguments.m_bodyName;
                        break;
 		}
 		case CMD_SYNC_USER_DATA_FAILED:

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -3463,8 +3463,6 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
 		btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
 
 		ser.startSerialization();
-		ser.registerNameForPointer(sb, bodyHandle->m_bodyName.c_str());
-
 		int len = sb->calculateSerializeBufferSize();
 		btChunk* chunk = ser.allocate(len, 1);
 		const char* structType = sb->serialize(chunk->m_oldPtr, &ser);
@@ -7259,6 +7257,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
 		serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
 		serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
 
+
 		serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
 			body->m_rootLocalInertialFrame.getOrigin()[0];
 		serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
@@ -7275,11 +7274,12 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
 		serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
 			body->m_rootLocalInertialFrame.getRotation()[3];
 
+    btVector3 center_of_mass(sb->getCenterOfMass());
 		btTransform tr = sb->getWorldTransform();
 		//base position in world space, cartesian
-		stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
+		stateDetails->m_actualStateQ[0] = center_of_mass[0];
-		stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
+		stateDetails->m_actualStateQ[1] = center_of_mass[1];
-		stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
+		stateDetails->m_actualStateQ[2] = center_of_mass[2];
 
 		//base orientation, quaternion x,y,z,w, in world space, cartesian
 		stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
@@ -8227,6 +8227,12 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 
 	    serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
 	    serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
+      int pos = strlen(relativeFileName)-1;
+      while(pos>=0 && relativeFileName[pos]!='/') { pos--;}
+      btAssert(strlen(relativeFileName)-pos-5>0);
+      std::string object_name (std::string(relativeFileName).substr(pos+1, strlen(relativeFileName)- 5 - pos));
+      bodyHandle->m_bodyName = object_name;
+
 
 	    int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
 	    serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
@@ -9338,6 +9344,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 	{
 		m_data->m_dynamicsWorld->getSolverInfo().m_warmstartingFactor = clientCmd.m_physSimParamArgs.m_warmStartingFactor;
 	}
+
+	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR)
+	{
+		m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_USE_ARTICULATED_WARMSTARTING;
+		m_data->m_dynamicsWorld->getSolverInfo().m_articulatedWarmstartingFactor = clientCmd.m_physSimParamArgs.m_articulatedWarmStartingFactor;
+	}
 	SharedMemoryStatus& serverCmd = serverStatusOut;
 	serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
 	return hasStatus;

examples/SharedMemory/SharedMemoryCommands.h

@@ -483,6 +483,7 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 1 << 25,
 	SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS = 1 << 26,
 	SIM_PARAM_UPDATE_WARM_STARTING_FACTOR = 1 << 27,
+	SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR = 1 << 28,
         
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -947,6 +947,7 @@ struct b3PhysicsSimulationParameters
 	int m_numSimulationSubSteps;
 	int m_numSolverIterations;
 	double m_warmStartingFactor;
+	double m_articulatedWarmStartingFactor;
 	int m_useRealTimeSimulation;
 	int m_useSplitImpulse;
 	double m_splitImpulsePenetrationThreshold;

examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_extended_env.py

@@ -122,7 +122,7 @@ class MinitaurExtendedEnv(MinitaurReactiveEnv):
     leg_model = []
     if self._include_leg_model:
       raw_motor_angles = self.minitaur.GetMotorAngles()
-      leg_model = self._convert_to_leg_model(raw_motor_angles)
+      leg_model = self.convert_to_leg_model(raw_motor_angles)
 
     observation_list = (
         [parent_observation] + history_states + history_actions +
@@ -185,7 +185,7 @@ class MinitaurExtendedEnv(MinitaurReactiveEnv):
     if self._never_terminate:
       return False
 
-    leg_model = self._convert_to_leg_model(self.minitaur.GetMotorAngles())
+    leg_model = self.convert_to_leg_model(self.minitaur.GetMotorAngles())
     swing0 = leg_model[0]
     swing1 = leg_model[2]
     maximum_swing_angle = 0.8

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -55,6 +55,7 @@ public:
 		: m_userPersistentData(0),
 		  m_contactPointFlags(0),
 		  m_appliedImpulse(0.f),
+		  m_prevRHS(0.f),
 		  m_appliedImpulseLateral1(0.f),
 		  m_appliedImpulseLateral2(0.f),
 		  m_contactMotion1(0.f),
@@ -79,6 +80,7 @@ public:
 										 m_userPersistentData(0),
 										 m_contactPointFlags(0),
 										 m_appliedImpulse(0.f),
+										 m_prevRHS(0.f),
 										 m_appliedImpulseLateral1(0.f),
 										 m_appliedImpulseLateral2(0.f),
 										 m_contactMotion1(0.f),
@@ -114,6 +116,7 @@ public:
 	int m_contactPointFlags;
 
 	btScalar m_appliedImpulse;
+	btScalar m_prevRHS;
 	btScalar m_appliedImpulseLateral1;
 	btScalar m_appliedImpulseLateral2;
 	btScalar m_contactMotion1;

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

@@ -325,6 +325,7 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma
 	{
 		const btManifoldPoint& pt = manifold->getContactPoint(i);
 		dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
+		dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS;
 		dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
 		dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
 		pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
@@ -371,6 +372,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleDa
 		btManifoldPoint& pt = m_pointCache[i];
 
 		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
 		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
 		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
 		pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
@@ -416,6 +418,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatDat
 		btManifoldPoint& pt = m_pointCache[i];
 
 		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
 		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
 		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
 		pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -173,6 +173,7 @@ public:
 			//get rid of duplicated userPersistentData pointer
 			m_pointCache[lastUsedIndex].m_userPersistentData = 0;
 			m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
+			m_pointCache[lastUsedIndex].m_prevRHS = 0.f;
 			m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
@@ -195,6 +196,7 @@ public:
 #ifdef MAINTAIN_PERSISTENCY
 		int lifeTime = m_pointCache[insertIndex].getLifeTime();
 		btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
+		btScalar prevRHS = m_pointCache[insertIndex].m_prevRHS;
 		btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
 		btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
 
@@ -223,6 +225,7 @@ public:
 			m_pointCache[insertIndex] = newPoint;
 			m_pointCache[insertIndex].m_userPersistentData = cache;
 			m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
+			m_pointCache[insertIndex].m_prevRHS = prevRHS;
 			m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
 			m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
 		}
@@ -276,6 +279,7 @@ struct btPersistentManifoldDoubleData
 	btVector3DoubleData	m_pointCacheLateralFrictionDir2[4];
 	double m_pointCacheDistance[4];
 	double m_pointCacheAppliedImpulse[4];
+	double m_pointCachePrevRHS[4];
 	 double m_pointCacheCombinedFriction[4];
 	double m_pointCacheCombinedRollingFriction[4];
 	double m_pointCacheCombinedSpinningFriction[4];
@@ -322,6 +326,7 @@ struct btPersistentManifoldFloatData
 	btVector3FloatData	m_pointCacheLateralFrictionDir2[4];
 	float m_pointCacheDistance[4];
 	float m_pointCacheAppliedImpulse[4];
+	float m_pointCachePrevRHS[4];
 	float m_pointCacheCombinedFriction[4];
 	float m_pointCacheCombinedRollingFriction[4];
 	float m_pointCacheCombinedSpinningFriction[4];

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -30,7 +30,8 @@ enum btSolverMode
 	SOLVER_SIMD = 256,
 	SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
 	SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024,
-	SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048
+	SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048,
+	SOLVER_USE_ARTICULATED_WARMSTARTING = 4096,
 };
 
 struct btContactSolverInfoData
@@ -54,7 +55,7 @@ struct btContactSolverInfoData
 	btScalar m_splitImpulseTurnErp;
 	btScalar m_linearSlop;
 	btScalar m_warmstartingFactor;
-
+	btScalar m_articulatedWarmstartingFactor;
 	int m_solverMode;
 	int m_restingContactRestitutionThreshold;
 	int m_minimumSolverBatchSize;
@@ -89,6 +90,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_splitImpulseTurnErp = 0.1f;
 		m_linearSlop = btScalar(0.0);
 		m_warmstartingFactor = btScalar(0.85);
+		m_articulatedWarmstartingFactor = btScalar(0.85);
 		//m_solverMode =  SOLVER_USE_WARMSTARTING |  SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
 		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;  // | SOLVER_RANDMIZE_ORDER;
 		m_restingContactRestitutionThreshold = 2;              //unused as of 2.81
@@ -120,6 +122,7 @@ struct btContactSolverInfoDoubleData
 	double m_splitImpulseTurnErp;
 	double m_linearSlop;
 	double m_warmstartingFactor;
+	double m_articulatedWarmstartingFactor;
 	double m_maxGyroscopicForce;  ///it is only used for 'explicit' version of gyroscopic force
 	double m_singleAxisRollingFrictionThreshold;
 
@@ -150,16 +153,17 @@ struct btContactSolverInfoFloatData
 
 	float m_linearSlop;
 	float m_warmstartingFactor;
+	float m_articulatedWarmstartingFactor;
 	float m_maxGyroscopicForce;
-	float m_singleAxisRollingFrictionThreshold;
 
+	float m_singleAxisRollingFrictionThreshold;
 	int m_numIterations;
 	int m_solverMode;
 	int m_restingContactRestitutionThreshold;
-	int m_minimumSolverBatchSize;
 
+	int m_minimumSolverBatchSize;
 	int m_splitImpulse;
-	char m_padding[4];
+	
 };
 
 #endif  //BT_CONTACT_SOLVER_INFO

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -1436,8 +1436,6 @@ void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
 
 	worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
 
-	// Fill padding with zeros to appease msan.
-	memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding));
 	
 #ifdef BT_USE_DOUBLE_PRECISION
 	const char* structType = "btDynamicsWorldDoubleData";

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -342,40 +342,6 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra
 		solverConstraint.m_friction = 0.f;  //cp.m_combinedFriction;
 	}
 
-	///warm starting (or zero if disabled)
-	/*
-     if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-     {
-     solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-     
-     if (solverConstraint.m_appliedImpulse)
-     {
-     if (multiBodyA)
-     {
-     btScalar impulse = solverConstraint.m_appliedImpulse;
-     btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-     multiBodyA->applyDeltaVee(deltaV,impulse);
-     applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-     } else
-     {
-     if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-     }
-     if (multiBodyB)
-     {
-     btScalar impulse = solverConstraint.m_appliedImpulse;
-     btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-     multiBodyB->applyDeltaVee(deltaV,impulse);
-     applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-     } else
-     {
-     if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-     }
-     }
-     } else
-     */
-
 	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
 

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -22,6 +22,8 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
 #include "LinearMath/btQuickprof.h"
+#include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h"
+#include "LinearMath/btScalar.h"
 
 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
@@ -491,11 +493,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 	return deltaVel;
 }
 
-void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
+void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
-																  const btVector3& contactNormal,
-																  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																  btScalar& relaxation,
-																  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	BT_PROFILE("setupMultiBodyContactConstraint");
 	btVector3 rel_pos1;
@@ -781,48 +779,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 	}
 
-	///warm starting (or zero if disabled)
-	//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
-	if (/* DISABLES CODE */ (0)) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-	{
-		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-
-		if (solverConstraint.m_appliedImpulse)
-		{
-			if (multiBodyA)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVeeMultiDof(deltaV, impulse);
-
-				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
-			}
-			else
-			{
-				if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
-			}
-			if (multiBodyB)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVeeMultiDof(deltaV, impulse);
-				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
-			}
-			else
-			{
-				if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
-			}
-		}
-	}
-	else
-	{
-		solverConstraint.m_appliedImpulse = 0.f;
-	}
-
-	solverConstraint.m_appliedPushImpulse = 0.f;
-
 	{
 		btScalar positionalError = 0.f;
 		btScalar velocityError = restitution - rel_vel;  // * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
@@ -874,6 +830,54 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 		solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
 	}
+        
+	if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
+	{
+		if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS)
+		{
+			solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor;
+			if (solverConstraint.m_appliedImpulse < 0)
+				solverConstraint.m_appliedImpulse = 0;
+		}
+		else
+		{
+			solverConstraint.m_appliedImpulse = 0.f;
+		}
+
+		if (solverConstraint.m_appliedImpulse)
+		{
+			if (multiBodyA)
+			{
+				btScalar impulse = solverConstraint.m_appliedImpulse;
+				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+				multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
+
+				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
+			}
+			else
+			{
+				if (rb0)
+					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
+			}
+			if (multiBodyB)
+			{
+				btScalar impulse = solverConstraint.m_appliedImpulse;
+				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+				multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
+				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
+			}
+			else
+			{
+				if (rb1)
+					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
+			}
+		}
+	}
+	else
+	{
+		solverConstraint.m_appliedImpulse = 0.f;
+   	solverConstraint.m_appliedPushImpulse = 0.f;
+	}
 }
 
 void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
@@ -1130,7 +1134,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
 	}
 }
 
-btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	BT_PROFILE("addMultiBodyFrictionConstraint");
 	btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
@@ -1161,7 +1165,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
 
 	solverConstraint.m_originalContactPoint = &cp;
 
-	setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
+	setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
@@ -1297,7 +1301,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			solverConstraint.m_originalContactPoint = &cp;
 
 			bool isFriction = false;
-			setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp, infoGlobal, relaxation, isFriction);
+			setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
 
 			//			const btVector3& pos1 = cp.getPositionWorldOnA();
 			//			const btVector3& pos2 = cp.getPositionWorldOnB();
@@ -1371,13 +1375,13 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 				{
 					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
+					  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
 					}
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
@@ -1388,26 +1392,27 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			}
 			else
 			{
-				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
+				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
 
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
-
-				//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
-				//todo:
 				solverConstraint.m_appliedImpulse = 0.f;
 				solverConstraint.m_appliedPushImpulse = 0.f;
       }
 
 #endif  //ENABLE_FRICTION
 		}
+		else
+		{
+			// Reset quantities related to warmstart as 0.
+			cp.m_appliedImpulse = 0;
+			cp.m_prevRHS = 0;
+		}
 	}
 }
 
 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 {
-	//btPersistentManifold* manifold = 0;
-
 	for (int i = 0; i < numManifolds; i++)
 	{
 		btPersistentManifold* manifold = manifoldPtr[i];
@@ -1434,6 +1439,51 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 
 		c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal);
 	}
+
+	// Warmstart for noncontact constraints
+	if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
+	{
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
+		{
+			btMultiBodySolverConstraint& solverConstraint =
+				m_multiBodyNonContactConstraints[i];
+			solverConstraint.m_appliedImpulse =
+				solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
+				infoGlobal.m_articulatedWarmstartingFactor;
+
+			btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+			btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+			if (solverConstraint.m_appliedImpulse)
+			{
+				if (multiBodyA)
+				{
+					int ndofA = multiBodyA->getNumDofs() + 6;
+					btScalar* deltaV =
+						&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+					btScalar impulse = solverConstraint.m_appliedImpulse;
+					multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
+					applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
+				}
+				if (multiBodyB)
+				{
+					int ndofB = multiBodyB->getNumDofs() + 6;
+					btScalar* deltaV =
+						&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+					btScalar impulse = solverConstraint.m_appliedImpulse;
+					multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
+					applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
+				}
+			}
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
+		{
+			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
+			solverConstraint.m_appliedImpulse = 0;
+		}
+	}
 }
 
 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
@@ -1556,7 +1606,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 		writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
 	}
 
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+
 	{
 		BT_PROFILE("warm starting write back");
 		for (int j = 0; j < numPoolConstraints; j++)
@@ -1565,6 +1615,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 			btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
 			btAssert(pt);
 			pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
+ 		  pt->m_prevRHS = solverConstraint.m_rhs;
 			pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
 
 			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
@@ -1576,9 +1627,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 				pt->m_appliedImpulseLateral2 = 0;
 			}
 		}
-		
-			//do a callback here?
 	}
+
 #if 0
 	//multibody joint feedback
 	{

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -49,7 +49,7 @@ protected:
 
 	void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
 
-	btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+	btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
 
 	btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
 																		 btScalar combinedTorsionalFriction,
@@ -66,7 +66,9 @@ protected:
 
 	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint,
 										 const btVector3& contactNormal,
-										 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+                     const btScalar& appliedImpulse,
+										 btManifoldPoint& cp,
+                     const btContactSolverInfo& infoGlobal,
 										 btScalar& relaxation,
 										 bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
 
@@ -82,7 +84,6 @@ protected:
 	void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
 	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 	//	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-
 	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 	void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof);
 	void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime);

src/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -14,7 +14,6 @@
  */
 
 #include "btDeformableContactConstraint.h"
-
 /* ================   Deformable Node Anchor   =================== */
 btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
 : m_anchor(&a)
@@ -216,7 +215,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
     btVector3 impulse = m_contact->m_c0 * vr;
     const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
     btVector3 impulse_tangent = impulse - impulse_normal;
-    
     btVector3 old_total_tangent_dv = m_total_tangent_dv;
     // m_c2 is the inverse mass of the deformable node/face
     m_total_normal_dv -= impulse_normal * m_contact->m_c2;
@@ -236,13 +234,13 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
             // dynamic friction
             // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
             m_static = false;
-            if (m_total_tangent_dv.norm() < SIMD_EPSILON)
+            if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
             {
                 m_total_tangent_dv = btVector3(0,0,0);
             }
             else
             {
-                m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_c3;
+                m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
             }
             impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
         }
@@ -255,7 +253,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
     impulse = impulse_normal + impulse_tangent;
     // apply impulse to deformable nodes involved and change their velocities
     applyImpulse(impulse);
-    
     // apply impulse to the rigid/multibodies involved and change their velocities
     if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
     {
@@ -361,12 +358,32 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
     const btSoftBody::DeformableFaceRigidContact* contact = getContact();
     btVector3 dv = impulse * contact->m_c2;
     btSoftBody::Face* face = contact->m_face;
-    if (face->m_n[0]->m_im > 0)
+    
-        face->m_n[0]->m_v -= dv * contact->m_weights[0];
+    btVector3& v0 = face->m_n[0]->m_v;
-    if (face->m_n[1]->m_im > 0)
+    btVector3& v1 = face->m_n[1]->m_v;
-        face->m_n[1]->m_v -= dv * contact->m_weights[1];
+    btVector3& v2 = face->m_n[2]->m_v;
-    if (face->m_n[2]->m_im > 0)
+    const btScalar& im0 = face->m_n[0]->m_im;
-        face->m_n[2]->m_v -= dv * contact->m_weights[2];
+    const btScalar& im1 = face->m_n[1]->m_im;
+    const btScalar& im2 = face->m_n[2]->m_im;
+    if (im0 > 0)
+        v0 -= dv * contact->m_weights[0];
+    if (im1 > 0)
+        v1 -= dv * contact->m_weights[1];
+    if (im2 > 0)
+        v2 -= dv * contact->m_weights[2];
+    
+    // apply strain limiting to prevent undamped modes
+    btScalar m01 = (btScalar(1)/(im0 + im1));
+    btScalar m02 = (btScalar(1)/(im0 + im2));
+    btScalar m12 = (btScalar(1)/(im1 + im2));
+    
+    btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
+    btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
+    btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
+    
+    v0 += dv0;
+    v1 += dv1;
+    v2 += dv2;
 }
 
 /* ================   Face vs. Node   =================== */
@@ -449,13 +466,13 @@ btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
             // dynamic friction
             // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
             m_static = false;
-            if (m_total_tangent_dv.norm() < SIMD_EPSILON)
+            if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
             {
                 m_total_tangent_dv = btVector3(0,0,0);
             }
             else
             {
-                m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_friction;
+                m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction;
             }
             impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
         }
@@ -482,16 +499,33 @@ void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impuls
     }
     
     btSoftBody::Face* face = contact->m_face;
-    if (face->m_n[0]->m_im > 0)
+    btVector3& v0 = face->m_n[0]->m_v;
+    btVector3& v1 = face->m_n[1]->m_v;
+    btVector3& v2 = face->m_n[2]->m_v;
+    const btScalar& im0 = face->m_n[0]->m_im;
+    const btScalar& im1 = face->m_n[1]->m_im;
+    const btScalar& im2 = face->m_n[2]->m_im;
+    if (im0 > 0)
     {
-        face->m_n[0]->m_v -= dvb * contact->m_weights[0];
+        v0 -= dvb * contact->m_weights[0];
     }
-    if (face->m_n[1]->m_im > 0)
+    if (im1 > 0)
     {
-        face->m_n[1]->m_v -= dvb * contact->m_weights[1];
+        v1 -= dvb * contact->m_weights[1];
     }
-    if (face->m_n[2]->m_im > 0)
+    if (im2 > 0)
     {
-        face->m_n[2]->m_v -= dvb * contact->m_weights[2];
+        v2 -= dvb * contact->m_weights[2];
     }
+    // todo: Face node constraints needs more work
+//    btScalar m01 = (btScalar(1)/(im0 + im1));
+//    btScalar m02 = (btScalar(1)/(im0 + im2));
+//    btScalar m12 = (btScalar(1)/(im1 + im2));
+//
+//    btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
+//    btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
+//    btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
+//    v0 += dv0;
+//    v1 += dv1;
+//    v2 += dv2;
 }

src/BulletSoftBody/btDeformableNeoHookeanForce.h

@@ -176,24 +176,30 @@ public:
                 btMatrix3x3 P;
                 firstPiola(psb->m_tetraScratches[j],P);
 #if USE_SVD
+                if (max_p > 0)
+                {
+                    // since we want to clamp the principal stress to max_p, we only need to
+                    // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
+                    btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
+                    if (trPTP > max_p * max_p)
+                    {
                         btMatrix3x3 U, V;
                         btVector3 sigma;
                         singularValueDecomposition(P, U, sigma, V);
-                if (max_p > 0)
-                {
                         sigma[0] = btMin(sigma[0], max_p);
                         sigma[1] = btMin(sigma[1], max_p);
                         sigma[2] = btMin(sigma[2], max_p);
                         sigma[0] = btMax(sigma[0], -max_p);
                         sigma[1] = btMax(sigma[1], -max_p);
                         sigma[2] = btMax(sigma[2], -max_p);
-                }
                         btMatrix3x3 Sigma;
                         Sigma.setIdentity();
                         Sigma[0][0] = sigma[0];
                         Sigma[1][1] = sigma[1];
                         Sigma[2][2] = sigma[2];
                         P = U * Sigma * V.transpose();
+                    }
+                }
 #endif
 //                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
                 btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();

src/BulletSoftBody/btSoftBody.h

@@ -20,6 +20,7 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btVector3.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
@@ -973,6 +974,16 @@ public:
 	/* Return the volume													*/
 	btScalar getVolume() const;
 	/* Cluster count														*/
+	btVector3 getCenterOfMass() const
+	{
+		btVector3 com(0, 0, 0);
+		for (int i = 0; i < m_nodes.size(); i++)
+		{
+			com += (m_nodes[i].m_x * this->getMass(i));
+		}
+		com /= this->getTotalMass();
+		return com;
+	}
 	int clusterCount() const;
 	/* Cluster center of mass												*/
 	static btVector3 clusterCom(const Cluster* cluster);

src/LinearMath/btImplicitQRSVD.h

@@ -582,6 +582,7 @@ inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btSca
 		// T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
 		return mu;
 	}
+	return a2;
 }
 
 /**

src/LinearMath/btScalar.h

@@ -25,13 +25,23 @@ subject to the following restrictions:
 #include <float.h>
 
 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
-#define BT_BULLET_VERSION 288
+#define BT_BULLET_VERSION 289
 
 inline int btGetVersion()
 {
 	return BT_BULLET_VERSION;
 }
 
+inline int btIsDoublePrecision()
+{
+  #ifdef BT_USE_DOUBLE_PRECISION
+  return true;
+  #else
+  return false;
+  #endif
+}
+
+
 // The following macro "BT_NOT_EMPTY_FILE" can be put into a file
 // in order suppress the MS Visual C++ Linker warning 4221
 //

src/LinearMath/btSerializer.h

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