From 5a3c60c7094395b64a2d82e6aa4161b766c997b2 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 9 Sep 2019 14:56:26 -0700 Subject: [PATCH] PyBullet: Fix syncBodyInfo for over 512 bodies. PyBullet: Fix issue related to recent change in drawDebugDrawerLines (soft body) --- .../PhysicsClientSharedMemory.cpp | 40 ++++++++++--- examples/SharedMemory/PhysicsDirect.cpp | 56 +++++++++++++++++-- .../PhysicsServerCommandProcessor.cpp | 41 ++++++++------ .../SharedMemory/PhysicsServerExample.cpp | 13 +++-- 4 files changed, 117 insertions(+), 33 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 71e1a1985..479fae693 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1529,19 +1529,45 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { B3_PROFILE("CMD_LOADING_COMPLETED"); int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints; - for (int i = 0; i < numConstraints; i++) - { - int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i]; - m_data->m_constraintIdsRequestInfo.push_back(constraintUid); - } int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + if (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED) + { + int* ids = (int*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; + int* constraintUids = ids + numBodies; + for (int i = 0; i < numConstraints; i++) + { + int constraintUid = constraintUids[i]; + m_data->m_constraintIdsRequestInfo.push_back(constraintUid); + } + } + else + { + for (int i = 0; i < numConstraints; i++) + { + int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i]; + m_data->m_constraintIdsRequestInfo.push_back(constraintUid); + } + } + if (numBodies > 0) { m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus; - for (int i = 0; i < numBodies; i++) + if (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED) { - m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]); + int* bodyIds = (int*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; + + for (int i = 0; i < numBodies; i++) + { + m_data->m_bodyIdsRequestInfo.push_back(bodyIds[i]); + } + } + else + { + for (int i = 0; i < numBodies; i++) + { + m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]); + } } int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size() - 1]; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index ba0aa9508..b8a4eb57c 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -688,7 +688,10 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta { bf.setFileDNAisMemoryDNA(); } - bf.parse(false); + { + BT_PROFILE("bf.parse"); + bf.parse(false); + } BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints); @@ -718,7 +721,8 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta bodyJoints->m_baseName = mb->m_baseName; } addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput); - } + + } } if (bf.ok()) { @@ -919,17 +923,57 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd break; } case CMD_SYNC_BODY_INFO_COMPLETED: - clearCachedBodies(); case CMD_MJCF_LOADING_COMPLETED: case CMD_SDF_LOADING_COMPLETED: { //we'll stream further info from the physics server //so serverCmd will be invalid, make a copy + btAlignedObjectArray bodyIdArray; + btAlignedObjectArray constraintIdArray; + int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints; + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + + bodyIdArray.reserve(numBodies); + constraintIdArray.reserve(numConstraints); + + if (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED) + { + clearCachedBodies(); + const int* bodyIds = (int*)m_data->m_bulletStreamDataServerToClient; + const int* constaintIds = bodyIds + numBodies; + + for (int i = 0; i < numConstraints; i++) + { + int constraintUid = constaintIds[i]; + constraintIdArray.push_back(constraintUid); + } + for (int i = 0; i < numBodies; i++) + { + int bodyUid = bodyIds[i]; + bodyIdArray.push_back(bodyUid); + } + } + else + { + for (int i = 0; i < numConstraints; i++) + { + int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i]; + constraintIdArray.push_back(constraintUid); + } + for (int i = 0; i < numBodies; i++) + { + int bodyUid = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]; + bodyIdArray.push_back(bodyUid); + } + + } + + for (int i = 0; i < numConstraints; i++) { - int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i]; + int constraintUid = constraintIdArray[i]; m_data->m_tmpInfoRequestCommand.m_type = CMD_USER_CONSTRAINT; m_data->m_tmpInfoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO; @@ -953,10 +997,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } } - int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + for (int i = 0; i < numBodies; i++) { - int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]; + int bodyUniqueId = bodyIdArray[i]; m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO; m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index bed051fb1..7ae098042 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5996,6 +5996,8 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar b3AlignedObjectArray usedHandles; m_data->m_bodyHandles.getUsedHandles(usedHandles); int actualNumBodies = 0; + int* bodyIds = (int*) bufferServerToClient; + for (int i = 0; i < usedHandles.size(); i++) { int usedHandle = usedHandles[i]; @@ -6006,18 +6008,19 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar if (body && (body->m_multiBody || body->m_rigidBody)) #endif { - serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle; + bodyIds[actualNumBodies++] = usedHandle; } } serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies; int usz = m_data->m_userConstraints.size(); serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz; + int* constraintIds = bodyIds + actualNumBodies; for (int i = 0; i < usz; i++) { int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1(); - // int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId; - serverStatusOut.m_sdfLoadedArgs.m_userConstraintUniqueIds[i] = key; + //int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId; + constraintIds[i] = key; } serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED; @@ -7755,8 +7758,11 @@ bool PhysicsServerCommandProcessor::processRequestBodyInfoCommand(const struct S const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; //stream info into memory - int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - + { + BT_PROFILE("createBodyInfoStream"); + int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + } serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0; @@ -7767,7 +7773,7 @@ bool PhysicsServerCommandProcessor::processRequestBodyInfoCommand(const struct S strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, bodyHandle->m_bodyName.c_str()); } - serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + return hasStatus; } @@ -12830,16 +12836,19 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags) m_data->m_guiHelper->render(m_data->m_dynamicsWorld); #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_DEFORMABLE_BODY - for (int i = 0; i < m_data->m_dynamicsWorld->getSoftBodyArray().size(); i++) - { - btSoftBody* psb = (btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i]; - { - btSoftBodyHelpers::DrawFrame(psb, m_data->m_dynamicsWorld->getDebugDrawer()); - btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), m_data->m_dynamicsWorld->getDrawFlags()); - } - } - m_data->m_guiHelper->drawDebugDrawerLines(); - m_data->m_guiHelper->clearLines(); + if (m_data->m_dynamicsWorld->getSoftBodyArray().size()) + { + for (int i = 0; i < m_data->m_dynamicsWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, m_data->m_dynamicsWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), m_data->m_dynamicsWorld->getDrawFlags()); + } + } + m_data->m_guiHelper->drawDebugDrawerLines(); + m_data->m_guiHelper->clearLines(); + } #endif #endif } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 0c0640884..c98500a10 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -654,16 +654,21 @@ public: { if (m_debugDraw) { + m_csGUI->lock(); + //draw stuff and flush? m_debugDraw->drawDebugDrawerLines(); + m_csGUI->unlock(); } } virtual void clearLines() { - if (m_debugDraw) - { - m_debugDraw->clearLines(); + m_csGUI->lock(); + if (m_debugDraw) + { + m_debugDraw->clearLines(); + } + m_csGUI->unlock(); } - } GUIHelperInterface* m_childGuiHelper;