AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
erwincoumans 2017-10-05 08:23:10 -07:00
parent a2d6a2e822
commit f467f325f7
11 changed files with 129 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/PhysicsClient.h
View File

@ -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;

12
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -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;

33
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@ -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();

2
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@ -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;

29
examples/SharedMemory/PhysicsDirect.cpp
View File

@ -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;

2
examples/SharedMemory/PhysicsDirect.h
View File

@ -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();

5
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@ -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);

2
examples/SharedMemory/PhysicsLoopBack.h
View File

@ -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;
};

63
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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;
}
}
}
}

1
examples/SharedMemory/SharedMemoryCommands.h
View File

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

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -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