Add calculateMass to shared memory interface.

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -3377,6 +3377,50 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i
     return true;
 }
 
+B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ)
+{
+    PhysicsClient* cl = (PhysicsClient*)physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    
+    command->m_type = CMD_CALCULATE_MASS_MATRIX;
+    command->m_updateFlags = 0;
+    int numJoints = cl->getNumJoints(bodyIndex);
+    for (int i = 0; i < numJoints; i++)
+    {
+        command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
+    }
+    
+    return (b3SharedMemoryCommandHandle)command;
+}
+
+
+B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix)
+{
+    const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+    btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
+    if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
+        return false;
+    
+    if (dofCount)
+    {
+    	*dofCount = status->m_massMatrixResultArgs.m_dofCount;
+    }
+    if (massMatrix)
+    {
+    	int numElements = status->m_massMatrixResultArgs.m_dofCount * status->m_massMatrixResultArgs.m_dofCount;
+        for (int i = 0; i < numElements; i++)
+        {
+            massMatrix[i] = status->m_massMatrixResultArgs.m_massMatrix[i];
+        }
+
+    }
+    
+    return true;
+}
+
 ///compute the joint positions to move the end effector to a desired target using inverse kinematics
 B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -310,19 +310,24 @@ B3_SHARED_API	void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
 ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
 	const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
-
 B3_SHARED_API	int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
 	int* bodyUniqueId,
 	int* dofCount,
 	double* jointForces);
 
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
-
 B3_SHARED_API	int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
 	int* dofCount,
 	double* linearJacobian,
 	double* angularJacobian);
 
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ);
+B3_SHARED_API	int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle,
+	int* dofCount,
+	double* massMatrix);
+
+
+
 ///compute the joint positions to move the end effector to a desired target using inverse kinematics
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
 B3_SHARED_API	void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);

examples/SharedMemory/SharedMemoryCommands.h

@@ -638,6 +638,18 @@ struct CalculateJacobianResultArgs
     double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];
 };
 
+struct CalculateMassMatrixArgs
+{
+	int m_bodyUniqueId;
+	double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CalculateMassMatrixResultArgs
+{
+	int m_dofCount;
+	double m_massMatrix[MAX_DEGREE_OF_FREEDOM * MAX_DEGREE_OF_FREEDOM];
+};
+
 enum EnumCalculateInverseKinematicsFlags
 {
     IK_HAS_TARGET_POSITION=1,
@@ -969,6 +981,7 @@ struct SharedMemoryCommand
         struct ExternalForceArgs m_externalForceArguments;
 		struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
         struct CalculateJacobianArgs m_calculateJacobianArguments;
+        struct CalculateMassMatrixArgs m_calculateMassMatrixArguments;
         struct b3UserConstraint m_userConstraintArguments;
         struct RequestContactDataArgs m_requestContactPointArguments;
 		struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
@@ -1043,6 +1056,7 @@ struct SharedMemoryStatus
 		struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
 		struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
         struct CalculateJacobianResultArgs m_jacobianResultArgs;
+        struct CalculateMassMatrixResultArgs m_massMatrixResultArgs;
 		struct SendContactDataArgs m_sendContactPointArgs;
 		struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs;
 		struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;

examples/SharedMemory/SharedMemoryPublic.h

@@ -43,6 +43,7 @@ enum EnumSharedMemoryClientCommand
 	CMD_CALCULATE_INVERSE_DYNAMICS,
     CMD_CALCULATE_INVERSE_KINEMATICS,
     CMD_CALCULATE_JACOBIAN,
+    CMD_CALCULATE_MASS_MATRIX,
     CMD_USER_CONSTRAINT,
     CMD_REQUEST_CONTACT_POINT_INFORMATION,
     CMD_REQUEST_RAY_CAST_INTERSECTIONS,
@@ -120,6 +121,8 @@ enum EnumSharedMemoryServerStatus
 		CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
         CMD_CALCULATED_JACOBIAN_COMPLETED,
         CMD_CALCULATED_JACOBIAN_FAILED,
+        CMD_CALCULATED_MASS_MATRIX_COMPLETED,
+        CMD_CALCULATED_MASS_MATRIX_FAILED,
 		CMD_CONTACT_POINT_INFORMATION_COMPLETED,
 		CMD_CONTACT_POINT_INFORMATION_FAILED,
 		CMD_REQUEST_AABB_OVERLAP_COMPLETED,