diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 28b823008..e81f25bfc 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/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; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 485d8c87d..c8f0ed604 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; - for (int i = 0; i < numElements; i++) - { - massMatrix[i] = status->m_massMatrixResultArgs.m_massMatrix[i]; - } - + cl->getCachedMassMatrix(status->m_massMatrixResultArgs.m_dofCount, massMatrix); } return true; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index e3f133acf..31d0c96d8 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -47,7 +47,7 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedVREvents; btAlignedObjectArray m_cachedKeyboardEvents; btAlignedObjectArray m_cachedMouseEvents; - + btAlignedObjectArray m_cachedMassMatrix; btAlignedObjectArray m_raycastHits; btAlignedObjectArray 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;im_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;im_cachedMassMatrix[i]; + } + } +} + + + + void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) { visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 31c717c02..54bb48817 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/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; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 94ca9bea6..7369cd43d 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -40,7 +40,7 @@ struct PhysicsDirectInternalData btHashMap m_userConstraintInfoMap; char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; - + btAlignedObjectArray m_cachedMassMatrix; int m_cachedCameraPixelsWidth; int m_cachedCameraPixelsHeight; btAlignedObjectArray 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;im_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;im_cachedMassMatrix[i]; + } + } +} + void PhysicsDirect::setTimeOut(double timeOutInSeconds) { m_data->m_timeOutInSeconds = timeOutInSeconds; diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 282a61732..a42ec01d8 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/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(); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 3067f2ace..e9880da7f 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/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); diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index f83cd7234..44d54a40e 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/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; }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f01cfa2ef..9fce685ae 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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) - { - for (int j = 0; j < (totDofs); ++j) - { - int element = (totDofs) * i + j; - serverCmd.m_massMatrixResultArgs.m_massMatrix[element] = massMatrix(i,j); - } - } - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; - } - else - { - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; + double* sharedBuf = (double*)bufferServerToClient; + int sizeInBytes = totDofs*totDofs*sizeof(double); + if (sizeInBytes < bufferSizeInBytes) + { + for (int i = 0; i < (totDofs); ++i) + { + for (int j = 0; j < (totDofs); ++j) + { + int element = (totDofs) * i + j; + + sharedBuf[element] = massMatrix(i,j); + } + } + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; + } } + } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 8466fd2e6..edc2052d8 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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 diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index d911b44c8..6a1bf33ec 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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