Merge pull request #1748 from erwincoumans/master

different way of sorting pairs,  exposed a few more methods in the C++ b3RobotSimulatorClientAPI

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -333,11 +333,12 @@ void ConvertURDF2BulletInternal(
 
         btRigidBody* linkRigidBody = 0;
         btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
+		bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0;
 
         if (!createMultiBody)
         {
             btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
-			bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0;
+			
 			if (!canSleep)
 			{
 				body->forceActivationState(DISABLE_DEACTIVATION);
@@ -365,7 +366,7 @@ void ConvertURDF2BulletInternal(
             if (cache.m_bulletMultiBody==0)
             {
                 
-                bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0;
+                
                 bool isFixedBase = (mass==0);//todo: figure out when base is fixed
                 int totalNumJoints = cache.m_totalNumJoints1;
                 cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep);
@@ -595,10 +596,13 @@ void ConvertURDF2BulletInternal(
 					}
                 } else
                 {
-					if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumLinks()==0)
+					if (canSleep)
 					{
-						//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
-						col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
+						if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumDofs()==0)
+						{
+							//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
+							col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
+						}
 					}
 					
 					

examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h

@@ -17,7 +17,7 @@ struct	GpuRigidBodyDemoInternalData
 
 	class b3GpuNarrowPhase* m_np;
 	class b3GpuBroadphaseInterface* m_bp;
-	class b3DynamicBvhBroadphase* m_broadphaseDbvt;
+	struct b3DynamicBvhBroadphase* m_broadphaseDbvt;
 
 	b3Vector3 m_pickPivotInA;
 	b3Vector3 m_pickPivotInB;

examples/RobotSimulator/HelloBulletRobotics.cpp

@@ -6,6 +6,8 @@ int main(int argc, char* argv[])
 	b3RobotSimulatorClientAPI_NoGUI* sim = new b3RobotSimulatorClientAPI_NoGUI();
 
 	bool isConnected = sim->connect(eCONNECT_SHARED_MEMORY);
+
+
 	if (!isConnected)
 	{
 		printf("Using Direct mode\n");
@@ -14,23 +16,36 @@ int main(int argc, char* argv[])
 	{
 		printf("Using shared memory\n");
 	}
+
+
 	//remove all existing objects (if any)
 	sim->resetSimulation();
+	sim->setGravity(btVector3(0,0,-9.8));
+	sim->setNumSolverIterations(100);
+	b3RobotSimulatorSetPhysicsEngineParameters args;
+	sim->getPhysicsEngineParameters(args);
 	
+
 	int planeUid = sim->loadURDF("plane.urdf");
 	printf("planeUid = %d\n", planeUid);
 	
 	int r2d2Uid = sim->loadURDF("r2d2.urdf");
 	printf("r2d2 #joints = %d\n", sim->getNumJoints(r2d2Uid));
-
-	btVector3 basePosition = btVector3(0,0,1);
+	
+	
+	btVector3 basePosition = btVector3(0,0,0.5);
 	btQuaternion baseOrientation = btQuaternion(0,0,0,1);
 
-	sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation);	
-	sim->setGravity(btVector3(0,0,-10));
+	sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation);
+	
 	
 	while (sim->isConnected())
 	{
+		btVector3 basePos;
+		btQuaternion baseOrn;
+		sim->getBasePositionAndOrientation(r2d2Uid,basePos,baseOrn);
+		printf("r2d2 basePosition = [%f,%f,%f]\n", basePos[0],basePos[1],basePos[2]);
+		
 		sim->stepSimulation();
 	}
 	delete sim;

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6859,24 +6859,45 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
 	bool hasStatus = true;
 	SharedMemoryStatus& serverCmd =serverStatusOut;
 	serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED;
+
+	serverCmd.m_simulationParameterResultArgs.m_allowedCcdPenetration = m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration;
 	serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode;
+	serverCmd.m_simulationParameterResultArgs.m_deltaTime =  m_data->m_physicsDeltaTime;
 	serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold;
+	serverCmd.m_simulationParameterResultArgs.m_contactSlop = m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop;
+	serverCmd.m_simulationParameterResultArgs.m_defaultGlobalCFM = m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm;
 	serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2;
 	serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp;
 	serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
+	serverCmd.m_simulationParameterResultArgs.m_deterministicOverlappingPairs = m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs;
+	serverCmd.m_simulationParameterResultArgs.m_enableConeFriction = (m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &SOLVER_DISABLE_IMPLICIT_CONE_FRICTION)? 0 : 1;
 	serverCmd.m_simulationParameterResultArgs.m_enableFileCaching =  b3IsFileCachingEnabled();
+	serverCmd.m_simulationParameterResultArgs.m_frictionCFM = m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM;
 	serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP;
 	btVector3 grav = m_data->m_dynamicsWorld->getGravity();
 	serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0];
 	serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1];
 	serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2];
 	serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags;
+	serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0;
+	if (gJointFeedbackInWorldSpace)
+	{
+		serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_WORLD_SPACE;
+	}
+	if (gJointFeedbackInJointFrame )
+	{
+		serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_JOINT_FRAME;
+	}
+	
 	serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps;
 	serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations;
 	serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold;
+	
+	serverCmd.m_simulationParameterResultArgs.m_solverResidualThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold;
 	serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold;
 	serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation;
 	serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse;
+	
 	return hasStatus;
 }
 
@@ -7010,7 +7031,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 
     if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM)
     {
-        m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionCFM;
+		m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM;
     }
 
 	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -224,6 +224,63 @@ bool b3RobotSimulatorClientAPI_NoDirect::loadMJCF(const std::string& fileName, b
 	return true;
 }
 
+bool b3RobotSimulatorClientAPI_NoDirect::savePythonWorld(const std::string& fileName)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+
+	if (fileName.length()==0)
+	{
+		return false;
+	}
+
+	command = b3SaveWorldCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_SAVE_WORLD_COMPLETED)
+	{
+		return false;
+	}
+	return true;
+}
+
+bool b3RobotSimulatorClientAPI_NoDirect::saveBullet(const std::string& fileName)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+
+	if (fileName.length()==0)
+	{
+		return false;
+	}
+
+	command = b3SaveBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_BULLET_SAVING_COMPLETED)
+	{
+		return false;
+	}
+	return true;
+}
+
+
 bool b3RobotSimulatorClientAPI_NoDirect::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
 {
 	if (!isConnected())
@@ -534,6 +591,19 @@ void b3RobotSimulatorClientAPI_NoDirect::removeConstraint(int constraintId)
 	b3GetStatusType(statusHandle);
 }
 
+bool b3RobotSimulatorClientAPI_NoDirect::getConstraintInfo(int constraintUniqueId, struct b3UserConstraint& constraintInfo)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}	
+	if (b3GetUserConstraintInfo(m_data->m_physicsClientHandle, constraintUniqueId, &constraintInfo))
+	{
+		return true;
+	}
+	return false;
+}
 
 bool b3RobotSimulatorClientAPI_NoDirect::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
 {
@@ -1478,8 +1548,34 @@ bool b3RobotSimulatorClientAPI_NoDirect::setJointMotorControlArray(int bodyUniqu
 	return true;
 }
 
+	
+bool b3RobotSimulatorClientAPI_NoDirect::getPhysicsEngineParameters(struct b3RobotSimulatorSetPhysicsEngineParameters &args)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
 
-bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args)
+	{
+		b3SharedMemoryCommandHandle command = b3InitRequestPhysicsParamCommand(sm);
+		b3SharedMemoryStatusHandle statusHandle;
+		int statusType;
+
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+		statusType = b3GetStatusType(statusHandle);
+		if (statusType!=CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED)
+		{
+			return false;
+		}
+		b3GetStatusPhysicsSimulationParameters(statusHandle,&args);
+	}
+	return true;
+}
+
+
+
+bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(const struct b3RobotSimulatorSetPhysicsEngineParameters &args)
 {
 	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
 	if (sm == 0) {
@@ -1497,12 +1593,12 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3Robo
 		b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode);
 	}
 
-	if (args.m_numSubSteps >= 0)	{
-		b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps);
+	if (args.m_numSimulationSubSteps >= 0)	{
+		b3PhysicsParamSetNumSubSteps(command, args.m_numSimulationSubSteps);
 	}
 
-	if (args.m_fixedTimeStep >= 0) {
-		b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep);
+	if (args.m_deltaTime >= 0) {
+		b3PhysicsParamSetTimeStep(command, args.m_deltaTime);
 	}
 
 	if (args.m_useSplitImpulse >= 0) {
@@ -1517,10 +1613,6 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3Robo
 		b3PhysicsParamSetContactBreakingThreshold(command, args.m_contactBreakingThreshold);
 	}
 
-	if (args.m_maxNumCmdPer1ms >= -1) {
-		b3PhysicsParamSetMaxNumCommandsPer1ms(command, args.m_maxNumCmdPer1ms);
-	}
-
 	if (args.m_restitutionVelocityThreshold>=0) {
 		b3PhysicsParamSetRestitutionVelocityThreshold(command, args.m_restitutionVelocityThreshold);
 	}
@@ -1529,12 +1621,12 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3Robo
 		b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching);
 	}
 
-	if (args.m_erp>=0) {
-		b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp);
+	if (args.m_defaultNonContactERP>=0) {
+		b3PhysicsParamSetDefaultNonContactERP(command,args.m_defaultNonContactERP);
 	}
 
-	if (args.m_contactERP>=0) {
-		b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP);
+	if (args.m_defaultContactERP>=0) {
+		b3PhysicsParamSetDefaultContactERP(command,args.m_defaultContactERP);
 	}
 
 	if (args.m_frictionERP >=0) {
@@ -2013,6 +2105,65 @@ bool b3RobotSimulatorClientAPI_NoDirect::getCollisionShapeData(int bodyUniqueId,
 	return true;
 }
 
+int b3RobotSimulatorClientAPI_NoDirect::saveStateToMemory()
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+	
+	int stateId = -1;
+
+	if (sm == 0)
+	{
+		return -1;
+	}
+
+	command = b3SaveStateCommandInit(sm);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType != CMD_SAVE_STATE_COMPLETED)
+	{
+		return -1;
+	}
+
+	stateId = b3GetStatusGetStateId(statusHandle);
+	return stateId;
+}
+
+void b3RobotSimulatorClientAPI_NoDirect::restoreStateFromMemory(int stateId)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return;
+	}
+
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	int physicsClientId = 0;
+	
+	command = b3LoadStateCommandInit(sm);
+	if (stateId >= 0)
+	{
+		b3LoadStateSetStateId(command, stateId);
+	}
+//	if (fileName)
+//	{
+//		b3LoadStateSetFileName(command, fileName);
+//	}
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+}
+
 bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo)
 {
 	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
@@ -2036,3 +2187,20 @@ bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3
 		return true;
 	}
 }
+
+void b3RobotSimulatorClientAPI_NoDirect::setAdditionalSearchPath(std::string path)
+{
+	int physicsClientId = 0;
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return;
+	}
+	if (path.length())
+	{
+		b3SharedMemoryCommandHandle commandHandle;
+		b3SharedMemoryStatusHandle statusHandle;
+		commandHandle = b3SetAdditionalSearchPath(sm, path.c_str());
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	}
+}

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h

@@ -200,39 +200,50 @@ struct b3RobotSimulatorGetCameraImageArgs
 	}
 };
 
-struct b3RobotSimulatorSetPhysicsEngineParameters 
-{
-	double m_fixedTimeStep;
-	int m_numSolverIterations;
-	int m_useSplitImpulse;
-	double m_splitImpulsePenetrationThreshold;
-	int m_numSubSteps;
-	int m_collisionFilterMode;
-	double m_contactBreakingThreshold;
-	int m_maxNumCmdPer1ms;
-	int m_enableFileCaching;
-	double m_restitutionVelocityThreshold;
-	double m_erp;
-	double m_contactERP;
-	double m_frictionERP;
-	double m_solverResidualThreshold;
 
+
+
+struct b3RobotSimulatorSetPhysicsEngineParameters : b3PhysicsSimulationParameters
+{
+	
 	b3RobotSimulatorSetPhysicsEngineParameters()
-		: m_fixedTimeStep(-1),
-		m_numSolverIterations(-1),
-		m_useSplitImpulse(-1),
-		m_splitImpulsePenetrationThreshold(-1),
-		m_numSubSteps(-1),
-		m_collisionFilterMode(-1),
-		m_contactBreakingThreshold(-1),
-		m_maxNumCmdPer1ms(-1),
-		m_enableFileCaching(-1),
-		m_restitutionVelocityThreshold(-1),
-		m_erp(-1),
-		m_contactERP(-1),
-		m_frictionERP(-1),
-	  	m_solverResidualThreshold(-1)
-	{}
+	{
+		
+		m_deltaTime=-1;
+		m_gravityAcceleration[3];
+		m_numSimulationSubSteps;
+		m_numSolverIterations=-1;
+		m_useRealTimeSimulation = -1;
+		m_useSplitImpulse = -1;
+		m_splitImpulsePenetrationThreshold =-1;
+		m_contactBreakingThreshold = -1;
+		m_internalSimFlags = -1;
+		m_defaultContactERP = -1;
+		m_collisionFilterMode = -1;
+		m_enableFileCaching = -1;
+		m_restitutionVelocityThreshold = -1;
+		m_defaultNonContactERP = -1;
+		m_frictionERP = -1;
+		m_defaultGlobalCFM = -1;
+		m_frictionCFM = -1;
+		m_enableConeFriction = -1;
+		m_deterministicOverlappingPairs = -1;
+		m_allowedCcdPenetration = -1;
+		m_jointFeedbackMode = -1;
+		m_solverResidualThreshold = -1;
+		m_contactSlop = -1;
+		
+		m_collisionFilterMode=-1;
+		m_contactBreakingThreshold=-1;
+		
+		m_enableFileCaching=-1;
+		m_restitutionVelocityThreshold=-1;
+		
+		m_frictionERP=-1;
+	  	m_solverResidualThreshold=-1;
+
+		
+	}
 };
 
 struct b3RobotSimulatorChangeDynamicsArgs
@@ -410,6 +421,54 @@ struct b3RobotSimulatorCreateMultiBodyArgs
 };
 
 
+struct b3RobotJointInfo : public b3JointInfo
+{
+	b3RobotJointInfo()
+	{
+		m_linkName[0] = 0;
+		m_jointName[0] = 0;
+		m_jointType = eFixedType;
+		m_qIndex = -1;
+		m_uIndex = -1;
+		m_jointIndex = -1;
+		m_flags = 0;
+		m_jointDamping = 0;
+		m_jointFriction = 0;
+		m_jointLowerLimit = 1;
+		m_jointUpperLimit = -1;
+		m_jointMaxForce = 500;
+		m_jointMaxVelocity = 100;
+		m_parentIndex = -1;
+
+		//position
+		m_parentFrame[0] = 0;
+		m_parentFrame[1] = 0;
+		m_parentFrame[2] = 0;
+		//orientation quaternion [x,y,z,w]
+		m_parentFrame[3] = 0;
+		m_parentFrame[4] = 0;
+		m_parentFrame[5] = 0;
+		m_parentFrame[6] = 1;
+
+				//position
+		m_childFrame[0] = 0;
+		m_childFrame[1] = 0;
+		m_childFrame[2] = 0;
+		//orientation quaternion [x,y,z,w]
+		m_childFrame[3] = 0;
+		m_childFrame[4] = 0;
+		m_childFrame[5] = 0;
+		m_childFrame[6] = 1;
+
+
+		m_jointAxis[0] = 0;
+		m_jointAxis[1] = 0;
+		m_jointAxis[2] = 1;
+	}
+};
+	
+
+
 class b3RobotSimulatorClientAPI_NoDirect
 {
 protected:
@@ -441,7 +500,10 @@ public:
 	bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs());
 	bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
 	bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
-
+	bool saveBullet(const std::string& fileName);
+	
+	bool savePythonWorld(const std::string& fileName);
+	
 	bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo);
 
 	bool getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const;
@@ -457,9 +519,11 @@ public:
 	int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
 
 	int changeConstraint(int constraintId, b3JointInfo* jointInfo);
-
+	
 	void removeConstraint(int constraintId);
 
+	bool getConstraintInfo(int constraintUniqueId, struct b3UserConstraint& constraintInfo);
+	
 	bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
 
 	bool getJointStates(int bodyUniqueId, b3JointStates2& state);
@@ -540,7 +604,9 @@ public:
 
 	bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args);
 
-	bool setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args);
+	bool setPhysicsEngineParameter(const struct b3RobotSimulatorSetPhysicsEngineParameters&args);
+	
+	bool getPhysicsEngineParameters(struct b3RobotSimulatorSetPhysicsEngineParameters&args);
 
 	bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags);
 
@@ -582,8 +648,15 @@ public:
 	bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo);
 
 	bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo);
-	
 
+	int saveStateToMemory();
+	void restoreStateFromMemory(int stateId);
+
+	int getAPIVersion() const
+	{
+		return SHARED_MEMORY_MAGIC_NUMBER;
+	}
+	void setAdditionalSearchPath(std::string path);
 };
 
 

src/BulletCollision/BroadphaseCollision/btDispatcher.h

@@ -47,7 +47,7 @@ struct btDispatcherInfo
 		m_allowedCcdPenetration(btScalar(0.04)),
 		m_useConvexConservativeDistanceUtil(false),
 		m_convexConservativeDistanceThreshold(0.0f),
-		m_deterministicOverlappingPairs(true)
+		m_deterministicOverlappingPairs(false)
 	{
 
 	}

src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

@@ -395,6 +395,70 @@ void	btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
 	}
 }
 
+struct MyPairIndex
+{
+    int m_orgIndex;
+    int m_uidA0;
+    int m_uidA1;
+};
+
+class MyPairIndeSortPredicate
+{
+public:
+    
+    bool operator() ( const MyPairIndex& a, const MyPairIndex& b ) const
+    {
+        const int uidA0 = a.m_uidA0;
+        const int uidB0 = b.m_uidA0;
+        const int uidA1 = a.m_uidA1;
+        const int uidB1 = b.m_uidA1;
+        return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
+    }
+};
+
+void    btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
+{
+    if (dispatchInfo.m_deterministicOverlappingPairs)
+    {
+        btBroadphasePairArray& pa = getOverlappingPairArray();
+        btAlignedObjectArray<MyPairIndex> indices;
+        {
+            BT_PROFILE("sortOverlappingPairs");
+            indices.resize(pa.size());
+            for (int i=0;i<indices.size();i++)
+            {
+                const btBroadphasePair& p = pa[i];
+                const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1;
+                const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1;
+                
+                indices[i].m_uidA0 = uidA0;
+                indices[i].m_uidA1 = uidA1;
+                indices[i].m_orgIndex = i;
+            }
+            indices.quickSort(MyPairIndeSortPredicate());
+        }
+        {
+            BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
+            int i;
+            for (i=0;i<indices.size();)
+            {
+                btBroadphasePair* pair = &pa[indices[i].m_orgIndex];
+                if (callback->processOverlap(*pair))
+                {
+                    removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
+                } else
+                {
+                    i++;
+                }
+            }
+        }
+    } else
+    {
+        processAllOverlappingPairs(callback, dispatcher);
+    }
+}
+
+
 void	btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
 {
 	///need to keep hashmap in sync with pair address, so rebuild all

src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

@@ -78,6 +78,10 @@ public:
 
 	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0;
 
+	virtual void    processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
+	{
+		processAllOverlappingPairs(callback, dispatcher, dispatchInfo);
+	}
 	virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0;
 
 	virtual bool	hasDeferredRemoval() = 0;
@@ -144,6 +148,8 @@ public:
 	
 	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
 
+    virtual void    processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo);
+
 	virtual btBroadphasePair*	getOverlappingPairArrayPtr()
 	{
 		return &m_overlappingPairArray[0];

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -246,20 +246,16 @@ public:
 
 
 
+
 void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) 
 {
 	//m_blockedForChanges = true;
 
 	btCollisionPairCallback	collisionCallback(dispatchInfo,this);
 
-	if (dispatchInfo.m_deterministicOverlappingPairs)
-	{
-		BT_PROFILE("sortOverlappingPairs");
-		pairCache->sortOverlappingPairs(this);
-	}
-	{
+    {
 		BT_PROFILE("processAllOverlappingPairs");
-		pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
+		pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher, dispatchInfo);
 	}
 
 	//m_blockedForChanges = false;