store mass matrix in streaming part of shared memory (no support for unlimited mass matrix sizes at the moment)

examples/SharedMemory/PhysicsClient.h

@@ -66,6 +66,8 @@ public:
 
 	virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits) = 0;
 
+	virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix) = 0;
+
 	virtual void setTimeOut(double timeOutInSeconds) = 0;
 	virtual double getTimeOut() const  = 0;
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -3417,8 +3417,11 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy
 }
 
 
-B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix)
+B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix)
 {
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+
     const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
     btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
     if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
@@ -3430,12 +3433,7 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle,
     }
     if (massMatrix)
     {
-    	int numElements = status->m_massMatrixResultArgs.m_dofCount * status->m_massMatrixResultArgs.m_dofCount;
+		cl->getCachedMassMatrix(status->m_massMatrixResultArgs.m_dofCount, massMatrix);
-        for (int i = 0; i < numElements; i++)
-        {
-            massMatrix[i] = status->m_massMatrixResultArgs.m_massMatrix[i];
-        }
-
     }
     
     return true;

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -47,7 +47,7 @@ struct PhysicsClientSharedMemoryInternalData {
 	btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
 	btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
 	btAlignedObjectArray<b3MouseEvent> m_cachedMouseEvents;
-	
+	btAlignedObjectArray<double> m_cachedMassMatrix;
 	btAlignedObjectArray<b3RayHitInfo>	m_raycastHits;
 
     btAlignedObjectArray<int> m_bodyIdsRequestInfo;
@@ -1210,6 +1210,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 				break;
 			}
 
+			case CMD_CALCULATED_MASS_MATRIX_FAILED:
+			{
+				b3Warning("calculate mass matrix failed");
+				break;
+			}
+			case CMD_CALCULATED_MASS_MATRIX_COMPLETED:
+			{
+				double* matrixData = (double*)&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
+				m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount);
+				for (int i=0;i<serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount;i++)
+				{
+					m_data->m_cachedMassMatrix[i] = matrixData[i];
+				}
+				break;
+			}
+
             default: {
                 b3Error("Unknown server status %d\n", serverCmd.m_type);
                 btAssert(0);
@@ -1531,6 +1547,21 @@ void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation
 }
 
 
+void PhysicsClientSharedMemory::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
+{
+	int sz = dofCountCheck*dofCountCheck;
+	if (sz == m_data->m_cachedMassMatrix.size())
+	{
+		for (int i=0;i<sz;i++)
+		{
+			massMatrix[i] = m_data->m_cachedMassMatrix[i];
+		}
+	}
+}
+
+
+
+
 void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
 {
 	visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -76,6 +76,8 @@ public:
 
 	virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
 
+	virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
+
 	virtual void setTimeOut(double timeOutInSeconds);
 	virtual double getTimeOut() const;
 

examples/SharedMemory/PhysicsDirect.cpp

@@ -40,7 +40,7 @@ struct PhysicsDirectInternalData
     btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
 	
 	char    m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
-
+	btAlignedObjectArray<double> m_cachedMassMatrix;
 	int m_cachedCameraPixelsWidth;
 	int m_cachedCameraPixelsHeight;
 	btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
@@ -951,6 +951,21 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		b3Warning("jacobian calculation failed");
 		break;
 	}
+	case CMD_CALCULATED_MASS_MATRIX_FAILED:
+		{
+			b3Warning("calculate mass matrix failed");
+			break;
+		}
+	case CMD_CALCULATED_MASS_MATRIX_COMPLETED:
+		{
+			double* matrixData = (double*)&m_data->m_bulletStreamDataServerToClient[0];
+			m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount);
+			for (int i=0;i<serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount;i++)
+			{
+				m_data->m_cachedMassMatrix[i] = matrixData[i];
+			}
+			break;
+		}
 	case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
 		{
 			break;
@@ -1220,6 +1235,18 @@ void PhysicsDirect::getCachedRaycastHits(struct b3RaycastInformation* raycastHit
 	raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0;
 }
 
+void PhysicsDirect::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
+{
+	int sz = dofCountCheck*dofCountCheck;
+	if (sz == m_data->m_cachedMassMatrix.size())
+	{
+		for (int i=0;i<sz;i++)
+		{
+			massMatrix[i] = m_data->m_cachedMassMatrix[i];
+		}
+	}
+}
+
 void PhysicsDirect::setTimeOut(double timeOutInSeconds)
 {
 	m_data->m_timeOutInSeconds = timeOutInSeconds;

examples/SharedMemory/PhysicsDirect.h

@@ -99,6 +99,8 @@ public:
 
 	virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
 
+	virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
+
 	//the following APIs are for internal use for visualization:
 	virtual bool connect(struct GUIHelperInterface* guiHelper);
 	virtual void renderScene();

examples/SharedMemory/PhysicsLoopBack.cpp

@@ -205,6 +205,11 @@ void PhysicsLoopBack::getCachedRaycastHits(struct b3RaycastInformation* raycastH
 	return m_data->m_physicsClient->getCachedRaycastHits(raycastHits);
 }
 
+void PhysicsLoopBack::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
+{
+	m_data->m_physicsClient->getCachedMassMatrix(dofCountCheck,massMatrix);
+}
+
 void PhysicsLoopBack::setTimeOut(double timeOutInSeconds)
 {
 	m_data->m_physicsClient->setTimeOut(timeOutInSeconds);

examples/SharedMemory/PhysicsLoopBack.h

@@ -80,6 +80,8 @@ public:
 
 	virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
 
+	virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
+
 	virtual void setTimeOut(double timeOutInSeconds);
 	virtual double getTimeOut() const;
 };

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -3003,6 +3003,38 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
 bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
 {
 //	BT_PROFILE("processCommand");
+	int szc = sizeof(SharedMemoryCommand);
+	int sz = sizeof(SharedMemoryStatus);
+
+	int sz1 = sizeof(BulletDataStreamArgs);
+		int sz2 = sizeof(SdfLoadedArgs);
+		int sz3 = sizeof(SendActualStateArgs);//30k
+		int sz4= sizeof(SendDebugLinesArgs);
+		int sz5= sizeof(SendPixelDataArgs);
+		int sz6 = sizeof(RigidBodyCreateArgs);
+		int sz7 = sizeof(CalculateInverseDynamicsResultArgs);
+        int sz8 = sizeof(CalculateJacobianResultArgs);
+        int sz9 = sizeof(CalculateMassMatrixResultArgs);//130k
+		int sz10 = sizeof(SendContactDataArgs);
+		int sz11 = sizeof(SendOverlappingObjectsArgs);
+		int sz12 = sizeof(CalculateInverseKinematicsResultArgs);
+		int sz13 = sizeof(SendVisualShapeDataArgs);
+		int sz14 = sizeof(UserDebugDrawResultArgs);
+		int sz15 = sizeof(b3UserConstraint);
+		int sz16 = sizeof(SendVREvents);
+		int sz17 = sizeof(SendKeyboardEvents);
+		int sz18 = sizeof(SendRaycastHits);
+		int sz19 = sizeof(StateLoggingResultArgs);
+		int sz20 = sizeof(b3OpenGLVisualizerCameraInfo);
+		int sz21 = sizeof(b3ObjectArgs);
+		int sz22 = sizeof(b3DynamicsInfo);
+		int sz23 = sizeof(b3CreateCollisionShapeResultArgs);
+		int sz24 = sizeof(b3CreateVisualShapeResultArgs);
+		int sz25 = sizeof(b3CreateMultiBodyResultArgs);
+		int sz26 = sizeof(b3SendCollisionInfoArgs);
+		int sz27 = sizeof(SendMouseEvents);
+		int sz28 = sizeof(b3LoadTextureResultArgs);
+		int sz29 = sizeof(b3CustomCommandResultArgs);
 
 	bool hasStatus = false;
 
@@ -6722,10 +6754,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					BT_PROFILE("CMD_CALCULATE_MASS_MATRIX");
 
                     SharedMemoryStatus& serverCmd = serverStatusOut;
+					serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
                     InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId);
                     if (bodyHandle && bodyHandle->m_multiBody)
                     {
-                        serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
                         
                         btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
                         
@@ -6744,20 +6776,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                             {
                                 serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs;
                                 // Fill in the result into the shared memory.
-                                for (int i = 0; i < (totDofs); ++i)
+								double* sharedBuf = (double*)bufferServerToClient;
-                                {
+								int sizeInBytes = totDofs*totDofs*sizeof(double);
-                                    for (int j = 0; j < (totDofs); ++j)
+								if (sizeInBytes < bufferSizeInBytes)
-                                    {
+								{
-                                        int element = (totDofs) * i + j;
+									for (int i = 0; i < (totDofs); ++i)
-                                        serverCmd.m_massMatrixResultArgs.m_massMatrix[element] = massMatrix(i,j);
+									{
-                                    }
+										for (int j = 0; j < (totDofs); ++j)
-                                }
+										{
-                                serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
+											int element = (totDofs) * i + j;
-                            }
+                                        
-                            else
+											sharedBuf[element] = massMatrix(i,j);
-                            {
+										}
-                                serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
+									}
+									serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
+								}
                             }
+                            
                         }
                         
                     }

examples/SharedMemory/SharedMemoryCommands.h

@@ -650,7 +650,6 @@ struct CalculateMassMatrixArgs
 struct CalculateMassMatrixResultArgs
 {
 	int m_dofCount;
-	double m_massMatrix[MAX_DEGREE_OF_FREEDOM * MAX_DEGREE_OF_FREEDOM];
 };
 
 enum EnumCalculateInverseKinematicsFlags

examples/SharedMemory/SharedMemoryPublic.h

@@ -5,7 +5,7 @@
 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
 ///my convention is year/month/day/rev
 
-#define SHARED_MEMORY_MAGIC_NUMBER 201709260
+#define SHARED_MEMORY_MAGIC_NUMBER 201710050
 //#define SHARED_MEMORY_MAGIC_NUMBER 201708270
 //#define SHARED_MEMORY_MAGIC_NUMBER 201707140
 //#define SHARED_MEMORY_MAGIC_NUMBER 201706015
@@ -575,6 +575,7 @@ enum b3ConfigureDebugVisualizerEnum
 	COV_ENABLE_SYNC_RENDERING_INTERNAL,
 	COV_ENABLE_KEYBOARD_SHORTCUTS,
 	COV_ENABLE_MOUSE_PICKING,
+	COV_ENABLE_Y_AXIS_UP,
 };
 
 enum b3AddUserDebugItemEnum