From adf31c8f644838c083248eb226dfa9f6b499e866 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 20 Feb 2019 19:57:10 -0800 Subject: [PATCH] remove PhysXClient.*, it was not needed (use PhysicsDirect instead) --- .../SharedMemory/PhysicsClientSharedMemory2.h | 6 +- examples/SharedMemory/physx/PhysXC_API.cpp | 5 +- examples/SharedMemory/physx/PhysXClient.cpp | 1409 ----------------- examples/SharedMemory/physx/PhysXClient.h | 135 -- examples/pybullet/premake4.lua | 2 - setup.py | 1 - 6 files changed, 6 insertions(+), 1552 deletions(-) delete mode 100644 examples/SharedMemory/physx/PhysXClient.cpp delete mode 100644 examples/SharedMemory/physx/PhysXClient.h diff --git a/examples/SharedMemory/PhysicsClientSharedMemory2.h b/examples/SharedMemory/PhysicsClientSharedMemory2.h index 70a9fd0a7..473ee0ec1 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory2.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory2.h @@ -1,5 +1,5 @@ -#ifndef PHYSICS_CLIENT_SHARED_MEMORY2_H -#define PHYSICS_CLIENT_SHARED_MEMORY2_H +#ifndef PHYSICS_CLIENT_SHARED_MEMORY3_H +#define PHYSICS_CLIENT_SHARED_MEMORY3_H #include "PhysicsDirect.h" @@ -14,4 +14,4 @@ public: void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem); }; -#endif //PHYSICS_CLIENT_SHARED_MEMORY2_H \ No newline at end of file +#endif //PHYSICS_CLIENT_SHARED_MEMORY3_H \ No newline at end of file diff --git a/examples/SharedMemory/physx/PhysXC_API.cpp b/examples/SharedMemory/physx/PhysXC_API.cpp index bd32027cf..d8706eb47 100644 --- a/examples/SharedMemory/physx/PhysXC_API.cpp +++ b/examples/SharedMemory/physx/PhysXC_API.cpp @@ -1,13 +1,14 @@ #ifdef BT_ENABLE_PHYSX #include "PhysXC_API.h" +#include "../PhysicsDirect.h" #include "PhysXServerCommandProcessor.h" -#include "PhysXClient.h" + B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX() { PhysXServerCommandProcessor* sdk = new PhysXServerCommandProcessor; - PhysXClient* direct = new PhysXClient(sdk, true); + PhysicsDirect* direct = new PhysicsDirect(sdk, true); bool connected; connected = direct->connect(); return (b3PhysicsClientHandle)direct; diff --git a/examples/SharedMemory/physx/PhysXClient.cpp b/examples/SharedMemory/physx/PhysXClient.cpp deleted file mode 100644 index 611c53125..000000000 --- a/examples/SharedMemory/physx/PhysXClient.cpp +++ /dev/null @@ -1,1409 +0,0 @@ -#ifdef BT_ENABLE_PHYSX -#include "PhysXClient.h" - -#include "../PhysicsClientSharedMemory.h" -#include "../../CommonInterfaces/CommonGUIHelperInterface.h" -#include "../SharedMemoryCommands.h" -#include "../PhysicsCommandProcessorInterface.h" -#include "../../Utils/b3Clock.h" - -#include "LinearMath/btHashMap.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "../../../Extras/Serialize/BulletFileLoader/btBulletFile.h" -#include "../../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" -#include "../BodyJointInfoUtility.h" -#include - -#include "../SharedMemoryUserData.h" -#include "LinearMath/btQuickprof.h" - -struct PhysXUserDataCache -{ - btHashMap m_userDataMap; - btHashMap m_keyToUserDataIdMap; - - ~PhysXUserDataCache() - { - } -}; - -struct BodyJointInfoCache2 -{ - std::string m_baseName; - btAlignedObjectArray m_jointInfo; - std::string m_bodyName; - - // Joint index -> user data. - btHashMap m_jointToUserDataMap; - - ~BodyJointInfoCache2() - { - } -}; - -struct PhysXDirectInternalData -{ - DummyGUIHelper m_noGfx; - - btAlignedObjectArray m_serverDNA; - SharedMemoryCommand m_command; - SharedMemoryStatus m_serverStatus; - - SharedMemoryCommand m_tmpInfoRequestCommand; - SharedMemoryStatus m_tmpInfoStatus; - bool m_hasStatus; - bool m_verboseOutput; - - btAlignedObjectArray m_debugLinesFrom; - btAlignedObjectArray m_debugLinesTo; - btAlignedObjectArray m_debugLinesColor; - - btHashMap m_bodyJointMap; - btHashMap m_userConstraintInfoMap; - - btAlignedObjectArray m_profileTimings; - btHashMap m_profileTimingStringArray; - - char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; - btAlignedObjectArray m_cachedMassMatrix; - int m_cachedCameraPixelsWidth; - int m_cachedCameraPixelsHeight; - btAlignedObjectArray m_cachedCameraPixelsRGBA; - btAlignedObjectArray m_cachedCameraDepthBuffer; - btAlignedObjectArray m_cachedSegmentationMask; - - btAlignedObjectArray m_cachedContactPoints; - btAlignedObjectArray m_cachedOverlappingObjects; - - btAlignedObjectArray m_cachedVisualShapes; - btAlignedObjectArray m_cachedCollisionShapes; - - btAlignedObjectArray m_cachedVREvents; - - btAlignedObjectArray m_cachedKeyboardEvents; - btAlignedObjectArray m_cachedMouseEvents; - - btAlignedObjectArray m_raycastHits; - - PhysicsCommandProcessorInterface* m_commandProcessor; - bool m_ownsCommandProcessor; - double m_timeOutInSeconds; - - PhysXDirectInternalData() - : m_hasStatus(false), - m_verboseOutput(false), - m_cachedCameraPixelsWidth(0), - m_cachedCameraPixelsHeight(0), - m_commandProcessor(NULL), - m_ownsCommandProcessor(false), - m_timeOutInSeconds(1e30) - { - memset(&m_command, 0, sizeof(m_command)); - memset(&m_serverStatus, 0, sizeof(m_serverStatus)); - memset(m_bulletStreamDataServerToClient, 0, sizeof(m_bulletStreamDataServerToClient)); - } -}; - -PhysXClient::PhysXClient(PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership) -{ - int sz = sizeof(SharedMemoryCommand); - int sz2 = sizeof(SharedMemoryStatus); - - m_data = new PhysXDirectInternalData; - m_data->m_commandProcessor = physSdk; - m_data->m_ownsCommandProcessor = passSdkOwnership; -} - -PhysXClient::~PhysXClient() -{ - for (int i = 0; i < m_data->m_profileTimingStringArray.size(); i++) - { - std::string** str = m_data->m_profileTimingStringArray.getAtIndex(i); - if (str) - { - delete *str; - } - } - m_data->m_profileTimingStringArray.clear(); - - if (m_data->m_commandProcessor->isConnected()) - { - m_data->m_commandProcessor->disconnect(); - } - if (m_data->m_ownsCommandProcessor) - { - delete m_data->m_commandProcessor; - } - - resetData(); - - delete m_data; -} - -void PhysXClient::resetData() -{ - m_data->m_debugLinesFrom.clear(); - m_data->m_debugLinesTo.clear(); - m_data->m_debugLinesColor.clear(); - for (int i = 0; i < m_data->m_bodyJointMap.size(); i++) - { - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); - if (bodyJointsPtr && *bodyJointsPtr) - { - delete (*bodyJointsPtr); - } - } - m_data->m_bodyJointMap.clear(); - m_data->m_userConstraintInfoMap.clear(); -} - -// return true if connection succesfull, can also check 'isConnected' -bool PhysXClient::connect() -{ - bool connected = m_data->m_commandProcessor->connect(); - m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); - - if (connected) - //also request serialization data - { - SharedMemoryCommand command; - command.m_type = CMD_REQUEST_INTERNAL_DATA; - bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - if (hasStatus) - { - postProcessStatus(m_data->m_serverStatus); - } - else - { - b3Clock clock; - double timeSec = clock.getTimeInSeconds(); - - while ((!hasStatus) && (clock.getTimeInSeconds() - timeSec < 10)) - { - const SharedMemoryStatus* stat = processServerStatus(); - if (stat) - { - hasStatus = true; - } - } - } - } - - return connected; -} - -// return true if connection succesfull, can also check 'isConnected' -bool PhysXClient::connect(struct GUIHelperInterface* guiHelper) -{ - bool connected = m_data->m_commandProcessor->connect(); - - m_data->m_commandProcessor->setGuiHelper(guiHelper); - - return connected; -} - -void PhysXClient::renderScene() -{ - int renderFlags = 0; - m_data->m_commandProcessor->renderScene(renderFlags); -} - -void PhysXClient::debugDraw(int debugDrawMode) -{ - m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode); -} - -////todo: rename to 'disconnect' -void PhysXClient::disconnectSharedMemory() -{ - m_data->m_commandProcessor->disconnect(); - m_data->m_commandProcessor->setGuiHelper(0); -} - -bool PhysXClient::isConnected() const -{ - return m_data->m_commandProcessor->isConnected(); -} - -// return non-null if there is a status, nullptr otherwise -const SharedMemoryStatus* PhysXClient::processServerStatus() -{ - if (!m_data->m_hasStatus) - { - m_data->m_hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - } - - SharedMemoryStatus* stat = 0; - if (m_data->m_hasStatus) - { - stat = &m_data->m_serverStatus; - - postProcessStatus(m_data->m_serverStatus); - - m_data->m_hasStatus = false; - } - return stat; -} - -SharedMemoryCommand* PhysXClient::getAvailableSharedMemoryCommand() -{ - return &m_data->m_command; -} - -bool PhysXClient::canSubmitCommand() const -{ - return m_data->m_commandProcessor->isConnected(); -} - -bool PhysXClient::processDebugLines(const struct SharedMemoryCommand& orgCommand) -{ - SharedMemoryCommand command = orgCommand; - - const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; - - do - { - bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - - b3Clock clock; - double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = m_data->m_timeOutInSeconds; - - while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) - { - const SharedMemoryStatus* stat = processServerStatus(); - if (stat) - { - hasStatus = true; - } - } - - m_data->m_hasStatus = hasStatus; - - if (hasStatus) - { - btAssert(m_data->m_serverStatus.m_type == CMD_DEBUG_LINES_COMPLETED); - - if (m_data->m_verboseOutput) - { - b3Printf("Success receiving %d debug lines", - serverCmd.m_sendDebugLinesArgs.m_numDebugLines); - } - - int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; - float* linesFrom = - (float*)&m_data->m_bulletStreamDataServerToClient[0]; - float* linesTo = - (float*)(&m_data->m_bulletStreamDataServerToClient[0] + - numLines * 3 * sizeof(float)); - float* linesColor = - (float*)(&m_data->m_bulletStreamDataServerToClient[0] + - 2 * numLines * 3 * sizeof(float)); - - m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + - numLines); - m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + - numLines); - m_data->m_debugLinesColor.resize( - serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines); - - for (int i = 0; i < numLines; i++) - { - TmpFloat3 from = CreateTmpFloat3(linesFrom[i * 3], linesFrom[i * 3 + 1], - linesFrom[i * 3 + 2]); - TmpFloat3 to = - CreateTmpFloat3(linesTo[i * 3], linesTo[i * 3 + 1], linesTo[i * 3 + 2]); - TmpFloat3 color = CreateTmpFloat3(linesColor[i * 3], linesColor[i * 3 + 1], - linesColor[i * 3 + 2]); - - m_data - ->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = - from; - m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = - to; - m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + - i] = color; - } - - if (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0) - { - m_data->m_hasStatus = false; - - command.m_type = CMD_REQUEST_DEBUG_LINES; - command.m_requestDebugLinesArguments.m_startingLineIndex = - serverCmd.m_sendDebugLinesArgs.m_numDebugLines + - serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; - } - } - - } while (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0); - - return m_data->m_hasStatus; -} - -bool PhysXClient::processVisualShapeData(const struct SharedMemoryCommand& orgCommand) -{ - SharedMemoryCommand command = orgCommand; - const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; - - do - { - bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - - b3Clock clock; - double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = m_data->m_timeOutInSeconds; - - while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) - { - const SharedMemoryStatus* stat = processServerStatus(); - if (stat) - { - hasStatus = true; - } - } - - m_data->m_hasStatus = hasStatus; - if (hasStatus) - { - if (m_data->m_verboseOutput) - { - b3Printf("Visual Shape Information Request OK\n"); - } - int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex; - int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; - m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied); - b3VisualShapeData* shapeData = (b3VisualShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; - for (int i = 0; i < numVisualShapesCopied; i++) - { - m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i]; - } - - if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied) - { - m_data->m_hasStatus = false; - - command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO; - command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; - command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId; - } - } - } while (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied); - - return m_data->m_hasStatus; -} - -bool PhysXClient::processOverlappingObjects(const struct SharedMemoryCommand& orgCommand) -{ - SharedMemoryCommand command = orgCommand; - - const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; - - do - { - bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - - b3Clock clock; - double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = m_data->m_timeOutInSeconds; - - while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) - { - const SharedMemoryStatus* stat = processServerStatus(); - if (stat) - { - hasStatus = true; - } - } - - m_data->m_hasStatus = hasStatus; - if (hasStatus) - { - if (m_data->m_verboseOutput) - { - b3Printf("Overlapping Objects Request OK\n"); - } - - int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex; - int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; - m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied); - b3OverlappingObject* objects = (b3OverlappingObject*)&m_data->m_bulletStreamDataServerToClient[0]; - - for (int i = 0; i < numOverlapCopied; i++) - { - m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i]; - } - - if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied) - { - m_data->m_hasStatus = false; - command.m_type = CMD_REQUEST_AABB_OVERLAP; - command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; - } - } - } while (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied); - - return m_data->m_hasStatus; -} - -bool PhysXClient::processContactPointData(const struct SharedMemoryCommand& orgCommand) -{ - SharedMemoryCommand command = orgCommand; - - const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; - - do - { - bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - - b3Clock clock; - double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = m_data->m_timeOutInSeconds; - - while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) - { - const SharedMemoryStatus* stat = processServerStatus(); - if (stat) - { - hasStatus = true; - } - } - - m_data->m_hasStatus = hasStatus; - if (hasStatus) - { - if (m_data->m_verboseOutput) - { - b3Printf("Contact Point Information Request OK\n"); - } - int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; - int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; - - m_data->m_cachedContactPoints.resize(startContactIndex + numContactsCopied); - - b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0]; - - for (int i = 0; i < numContactsCopied; i++) - { - m_data->m_cachedContactPoints[startContactIndex + i] = contactData[i]; - } - - if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) - { - m_data->m_hasStatus = false; - - command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; - command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; - command.m_requestContactPointArguments.m_objectAIndexFilter = -1; - command.m_requestContactPointArguments.m_objectBIndexFilter = -1; - } - } - } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied); - - return m_data->m_hasStatus; -} - -bool PhysXClient::processCamera(const struct SharedMemoryCommand& orgCommand) -{ - SharedMemoryCommand command = orgCommand; - - const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; - - do - { - bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - - b3Clock clock; - double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = m_data->m_timeOutInSeconds; - - while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) - { - const SharedMemoryStatus* stat = processServerStatus(); - if (stat) - { - hasStatus = true; - } - } - - m_data->m_hasStatus = hasStatus; - if (hasStatus) - { - btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED); - - if (m_data->m_verboseOutput) - { - b3Printf("Camera image OK\n"); - } - - int numBytesPerPixel = 4; //RGBA - int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + - serverCmd.m_sendPixelDataArguments.m_numPixelsCopied + - serverCmd.m_sendPixelDataArguments.m_numRemainingPixels; - - m_data->m_cachedCameraPixelsWidth = 0; - m_data->m_cachedCameraPixelsHeight = 0; - - int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth * serverCmd.m_sendPixelDataArguments.m_imageHeight; - - m_data->m_cachedCameraPixelsRGBA.reserve(numPixels * numBytesPerPixel); - m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels); - m_data->m_cachedSegmentationMask.resize(numTotalPixels); - m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels * numBytesPerPixel); - - unsigned char* rgbaPixelsReceived = - (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0]; - - float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * 4]); - int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * 8]); - - // printf("pixel = %d\n", rgbaPixelsReceived[0]); - - for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; i++) - { - m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; - } - for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; i++) - { - m_data->m_cachedSegmentationMask[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i]; - } - for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * numBytesPerPixel; i++) - { - m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex * numBytesPerPixel] = rgbaPixelsReceived[i]; - } - - if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) - { - m_data->m_hasStatus = false; - - // continue requesting remaining pixels - command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA; - command.m_requestPixelDataArguments.m_startPixelIndex = - serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + - serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; - } - else - { - m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth; - m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight; - } - } - } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied); - - return m_data->m_hasStatus; -} - -void PhysXClient::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) -{ - BodyJointInfoCache2** cachePtr = m_data->m_bodyJointMap[bodyUniqueId]; - //don't process same bodyUniqueId multiple times - if (cachePtr) - { - return; - } - - bParse::btBulletFile bf( - &m_data->m_bulletStreamDataServerToClient[0], - serverCmd.m_numDataStreamBytes); - if (m_data->m_serverDNA.size()) - { - bf.setFileDNA(false, &m_data->m_serverDNA[0], m_data->m_serverDNA.size()); - } - else - { - bf.setFileDNAisMemoryDNA(); - } - bf.parse(false); - - if (bf.ok()) - { - BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; - m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints); - bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; - - for (int i = 0; i < bf.m_multiBodies.size(); i++) - { - int flag = bf.getFlags(); - if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) - { - Bullet::btMultiBodyDoubleData* mb = - (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; - - if (mb->m_baseName) - { - bodyJoints->m_baseName = mb->m_baseName; - } - addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput); - } - else - { - Bullet::btMultiBodyFloatData* mb = - (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; - - if (mb->m_baseName) - { - bodyJoints->m_baseName = mb->m_baseName; - } - addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput); - } - } - - if (m_data->m_verboseOutput) - { - b3Printf("Received robot description ok!\n"); - } - } - else - { - b3Warning("Robot description not received"); - } -} - -void PhysXClient::processAddUserData(const struct SharedMemoryStatus& serverCmd) -{ - -} - -void PhysXClient::postProcessStatus(const struct SharedMemoryStatus& serverCmd) -{ - switch (serverCmd.m_type) - { - case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Raycast completed"); - } - m_data->m_raycastHits.clear(); - b3RayHitInfo* rayHits = (b3RayHitInfo*)m_data->m_bulletStreamDataServerToClient; - for (int i = 0; i < serverCmd.m_raycastHits.m_numRaycastHits; i++) - { - m_data->m_raycastHits.push_back(rayHits[i]); - } - break; - } - case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Request VR Events completed"); - } - m_data->m_cachedVREvents.resize(serverCmd.m_sendVREvents.m_numVRControllerEvents); - for (int i = 0; i < serverCmd.m_sendVREvents.m_numVRControllerEvents; i++) - { - m_data->m_cachedVREvents[i] = serverCmd.m_sendVREvents.m_controllerEvents[i]; - } - break; - } - case CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Request keyboard events completed"); - } - m_data->m_cachedKeyboardEvents.resize(serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents); - for (int i = 0; i < serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents; i++) - { - m_data->m_cachedKeyboardEvents[i] = serverCmd.m_sendKeyboardEvents.m_keyboardEvents[i]; - } - break; - } - - case CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED: - { - B3_PROFILE("CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED"); - if (m_data->m_verboseOutput) - { - b3Printf("Request mouse events completed"); - } - m_data->m_cachedMouseEvents.resize(serverCmd.m_sendMouseEvents.m_numMouseEvents); - for (int i = 0; i < serverCmd.m_sendMouseEvents.m_numMouseEvents; i++) - { - m_data->m_cachedMouseEvents[i] = serverCmd.m_sendMouseEvents.m_mouseEvents[i]; - } - break; - } - - case CMD_REQUEST_INTERNAL_DATA_COMPLETED: - { - if (serverCmd.m_numDataStreamBytes) - { - int numStreamBytes = serverCmd.m_numDataStreamBytes; - m_data->m_serverDNA.resize(numStreamBytes); - for (int i = 0; i < numStreamBytes; i++) - { - m_data->m_serverDNA[i] = m_data->m_bulletStreamDataServerToClient[i]; - } - } - break; - } - case CMD_RESET_SIMULATION_COMPLETED: - { - resetData(); - break; - } - - case CMD_USER_CONSTRAINT_INFO_COMPLETED: - case CMD_USER_CONSTRAINT_COMPLETED: - { - int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; - m_data->m_userConstraintInfoMap.insert(cid, serverCmd.m_userConstraintResultArgs); - break; - } - case CMD_REMOVE_USER_CONSTRAINT_COMPLETED: - { - int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; - m_data->m_userConstraintInfoMap.remove(cid); - break; - } - case CMD_REMOVE_BODY_FAILED: - { - b3Warning("Remove body failed\n"); - break; - } - case CMD_REMOVE_BODY_COMPLETED: - { - for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numBodies; i++) - { - int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i]; - removeCachedBody(bodyUniqueId); - } - for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numUserConstraints; i++) - { - int key = serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[i]; - m_data->m_userConstraintInfoMap.remove(key); - } - - break; - } - case CMD_CHANGE_USER_CONSTRAINT_COMPLETED: - { - int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; - b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid]; - if (userConstraintPtr) - { - const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs; - if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) - { - userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0]; - userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1]; - userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2]; - } - if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) - { - userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3]; - userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4]; - userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5]; - userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6]; - } - if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) - { - userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce; - } - if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) - { - userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio; - } - if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) - { - userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget; - } - if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) - { - userConstraintPtr->m_erp = serverConstraint->m_erp; - } - if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) - { - userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink; - } - } - break; - } - case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED: - { - break; - } - case CMD_SYNC_BODY_INFO_COMPLETED: - 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 - - 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_tmpInfoRequestCommand.m_type = CMD_USER_CONSTRAINT; - m_data->m_tmpInfoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO; - m_data->m_tmpInfoRequestCommand.m_userConstraintArguments.m_userConstraintUniqueId = constraintUid; - - bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - - b3Clock clock; - double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = m_data->m_timeOutInSeconds; - - while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) - { - hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - } - - if (hasStatus) - { - int cid = m_data->m_tmpInfoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId; - m_data->m_userConstraintInfoMap.insert(cid, m_data->m_tmpInfoStatus.m_userConstraintResultArgs); - } - } - - int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; - for (int i = 0; i < numBodies; i++) - { - int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]; - - m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO; - m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId; - - bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - - b3Clock clock; - double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = m_data->m_timeOutInSeconds; - - while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) - { - hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - } - - if (hasStatus) - { - processBodyJointInfo(bodyUniqueId, m_data->m_tmpInfoStatus); - } - } - break; - } - case CMD_CREATE_MULTI_BODY_COMPLETED: - case CMD_URDF_LOADING_COMPLETED: - { - if (serverCmd.m_numDataStreamBytes > 0) - { - int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; - processBodyJointInfo(bodyIndex, serverCmd); - } - break; - } - case CMD_BULLET_LOADING_FAILED: - { - b3Warning("Couldn't load .bullet file"); - break; - } - case CMD_BULLET_LOADING_COMPLETED: - { - break; - } - - case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: - { - break; - } - - case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED: - { - b3Warning("requestOpenGLVisualizeCamera failed"); - break; - } - case CMD_REMOVE_USER_CONSTRAINT_FAILED: - { - b3Warning("removeConstraint failed"); - break; - } - case CMD_CHANGE_USER_CONSTRAINT_FAILED: - { - //b3Warning("changeConstraint failed"); - break; - } - - case CMD_USER_CONSTRAINT_FAILED: - { - b3Warning("createConstraint failed"); - break; - } - - case CMD_CREATE_COLLISION_SHAPE_FAILED: - { - b3Warning("createCollisionShape failed"); - break; - } - case CMD_CREATE_COLLISION_SHAPE_COMPLETED: - { - break; - } - - case CMD_CREATE_VISUAL_SHAPE_FAILED: - { - b3Warning("createVisualShape failed"); - break; - } - case CMD_CREATE_VISUAL_SHAPE_COMPLETED: - { - break; - } - - case CMD_CREATE_MULTI_BODY_FAILED: - { - b3Warning("createMultiBody failed"); - break; - } - case CMD_REQUEST_COLLISION_INFO_COMPLETED: - { - break; - } - case CMD_REQUEST_COLLISION_INFO_FAILED: - { - b3Warning("Request getCollisionInfo failed"); - break; - } - - case CMD_CUSTOM_COMMAND_COMPLETED: - { - break; - } - case CMD_CUSTOM_COMMAND_FAILED: - { - b3Warning("custom plugin command failed"); - break; - } - case CMD_CLIENT_COMMAND_COMPLETED: - { - break; - } - case CMD_CALCULATED_JACOBIAN_COMPLETED: - { - break; - } - case CMD_CALCULATED_JACOBIAN_FAILED: - { - 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; - } - case CMD_DESIRED_STATE_RECEIVED_COMPLETED: - { - break; - } - case CMD_STEP_FORWARD_SIMULATION_COMPLETED: - { - break; - } - case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED: - { - break; - } - case CMD_SAVE_STATE_COMPLETED: - { - break; - } - case CMD_COLLISION_SHAPE_INFO_FAILED: - { - b3Warning("getCollisionShapeData failed"); - break; - } - case CMD_COLLISION_SHAPE_INFO_COMPLETED: - { - B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED"); - if (m_data->m_verboseOutput) - { - b3Printf("Collision Shape Information Request OK\n"); - } - int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes; - m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied); - b3CollisionShapeData* shapeData = (b3CollisionShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; - for (int i = 0; i < numCollisionShapesCopied; i++) - { - m_data->m_cachedCollisionShapes[i] = shapeData[i]; - } - break; - } - case CMD_RESTORE_STATE_FAILED: - { - b3Warning("restoreState failed"); - break; - } - case CMD_RESTORE_STATE_COMPLETED: - { - break; - } - case CMD_BULLET_SAVING_COMPLETED: - { - break; - } - case CMD_LOAD_SOFT_BODY_FAILED: - { - b3Warning("loadSoftBody failed"); - break; - } - case CMD_LOAD_SOFT_BODY_COMPLETED: - { - break; - } - case CMD_SYNC_USER_DATA_FAILED: - { - b3Warning("Synchronizing user data failed."); - break; - } - case CMD_ADD_USER_DATA_FAILED: - { - b3Warning("Adding user data failed (do the specified body and link exist?)"); - break; - } - case CMD_REMOVE_USER_DATA_FAILED: - { - b3Warning("Removing user data failed"); - break; - } - case CMD_ADD_USER_DATA_COMPLETED: - { - processAddUserData(serverCmd); - break; - } - case CMD_SYNC_USER_DATA_COMPLETED: - { - B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED"); - break; - } - case CMD_REMOVE_USER_DATA_COMPLETED: - { - break; - } - default: - { - //b3Warning("Unknown server status type"); - } - }; -} -bool PhysXClient::submitClientCommand(const struct SharedMemoryCommand& command) -{ - if (command.m_type == CMD_REQUEST_DEBUG_LINES) - { - return processDebugLines(command); - } - - if (command.m_type == CMD_REQUEST_CAMERA_IMAGE_DATA) - { - return processCamera(command); - } - if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION) - { - return processContactPointData(command); - } - - if (command.m_type == CMD_REQUEST_VISUAL_SHAPE_INFO) - { - return processVisualShapeData(command); - } - if (command.m_type == CMD_REQUEST_AABB_OVERLAP) - { - return processOverlappingObjects(command); - } - - bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - m_data->m_hasStatus = hasStatus; - /*if (hasStatus) - { - postProcessStatus(m_data->m_serverStatus); - m_data->m_hasStatus = false; - } - */ - return hasStatus; -} - -int PhysXClient::getNumBodies() const -{ - return m_data->m_bodyJointMap.size(); -} - -void PhysXClient::removeCachedBody(int bodyUniqueId) -{ - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (bodyJointsPtr && *bodyJointsPtr) - { - delete (*bodyJointsPtr); - m_data->m_bodyJointMap.remove(bodyUniqueId); - } -} - -int PhysXClient::getNumUserConstraints() const -{ - return m_data->m_userConstraintInfoMap.size(); -} - -int PhysXClient::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const -{ - b3UserConstraint* constraintPtr = m_data->m_userConstraintInfoMap[constraintUniqueId]; - if (constraintPtr) - { - info = *constraintPtr; - return 1; - } - return 0; -} - -int PhysXClient::getUserConstraintId(int serialIndex) const -{ - if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) - { - return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); - } - return -1; -} - -int PhysXClient::getBodyUniqueId(int serialIndex) const -{ - if ((serialIndex >= 0) && (serialIndex < getNumBodies())) - { - return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1(); - } - return -1; -} - -bool PhysXClient::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const -{ - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (bodyJointsPtr && *bodyJointsPtr) - { - BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - strcpy(info.m_baseName, bodyJoints->m_baseName.c_str()); - strcpy(info.m_bodyName, bodyJoints->m_bodyName.c_str()); - return true; - } - - return false; -} - -int PhysXClient::getNumJoints(int bodyIndex) const -{ - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; - if (bodyJointsPtr && *bodyJointsPtr) - { - BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - return bodyJoints->m_jointInfo.size(); - } - btAssert(0); - return 0; -} - -bool PhysXClient::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const -{ - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; - if (bodyJointsPtr && *bodyJointsPtr) - { - BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size())) - { - info = bodyJoints->m_jointInfo[jointIndex]; - return true; - } - } - return false; -} - -void PhysXClient::setSharedMemoryKey(int key) -{ -} - -void PhysXClient::uploadBulletFileToSharedMemory(const char* data, int len) -{ - if (len > SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) - { - len = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE; - } - for (int i = 0; i < len; i++) - { - m_data->m_bulletStreamDataServerToClient[i] = data[i]; - } - //m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); -} - -void PhysXClient::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) -{ - int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays; - int newNumRays = curNumStreamingRays + numRays; - btAssert(newNumRays < MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING); - - if (newNumRays < MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING) - { - for (int i = 0; i < numRays; i++) - { - b3RayData* rayDataStream = (b3RayData*)m_data->m_bulletStreamDataServerToClient; - rayDataStream[curNumStreamingRays + i].m_rayFromPosition[0] = rayFromWorldArray[i * 3 + 0]; - rayDataStream[curNumStreamingRays + i].m_rayFromPosition[1] = rayFromWorldArray[i * 3 + 1]; - rayDataStream[curNumStreamingRays + i].m_rayFromPosition[2] = rayFromWorldArray[i * 3 + 2]; - rayDataStream[curNumStreamingRays + i].m_rayToPosition[0] = rayToWorldArray[i * 3 + 0]; - rayDataStream[curNumStreamingRays + i].m_rayToPosition[1] = rayToWorldArray[i * 3 + 1]; - rayDataStream[curNumStreamingRays + i].m_rayToPosition[2] = rayToWorldArray[i * 3 + 2]; - command.m_requestRaycastIntersections.m_numStreamingRays++; - } - } -} - -int PhysXClient::getNumDebugLines() const -{ - return m_data->m_debugLinesFrom.size(); -} - -const float* PhysXClient::getDebugLinesFrom() const -{ - if (getNumDebugLines()) - { - return &m_data->m_debugLinesFrom[0].m_x; - } - return 0; -} -const float* PhysXClient::getDebugLinesTo() const -{ - if (getNumDebugLines()) - { - return &m_data->m_debugLinesTo[0].m_x; - } - return 0; -} -const float* PhysXClient::getDebugLinesColor() const -{ - if (getNumDebugLines()) - { - return &m_data->m_debugLinesColor[0].m_x; - } - return 0; -} - -void PhysXClient::getCachedCameraImage(b3CameraImageData* cameraData) -{ - if (cameraData) - { - cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; - cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; - cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; - cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; - cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size() ? &m_data->m_cachedSegmentationMask[0] : 0; - } -} - -void PhysXClient::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) -{ - contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); - contactPointData->m_contactPointData = contactPointData->m_numContactPoints ? &m_data->m_cachedContactPoints[0] : 0; -} - -void PhysXClient::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) -{ - overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size(); - overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ? &m_data->m_cachedOverlappingObjects[0] : 0; -} - -void PhysXClient::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) -{ - visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); - visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0; -} - -void PhysXClient::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo) -{ - collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size(); - collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0; -} - -void PhysXClient::getCachedVREvents(struct b3VREventsData* vrEventsData) -{ - vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size(); - vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents ? &m_data->m_cachedVREvents[0] : 0; -} - -void PhysXClient::getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) -{ - keyboardEventsData->m_numKeyboardEvents = m_data->m_cachedKeyboardEvents.size(); - keyboardEventsData->m_keyboardEvents = keyboardEventsData->m_numKeyboardEvents ? &m_data->m_cachedKeyboardEvents[0] : 0; -} - -void PhysXClient::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) -{ - mouseEventsData->m_numMouseEvents = m_data->m_cachedMouseEvents.size(); - mouseEventsData->m_mouseEvents = mouseEventsData->m_numMouseEvents ? &m_data->m_cachedMouseEvents[0] : 0; -} - -void PhysXClient::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) -{ - raycastHits->m_numRayHits = m_data->m_raycastHits.size(); - raycastHits->m_rayHits = raycastHits->m_numRayHits ? &m_data->m_raycastHits[0] : 0; -} - -void PhysXClient::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 PhysXClient::setTimeOut(double timeOutInSeconds) -{ - m_data->m_timeOutInSeconds = timeOutInSeconds; -} - -double PhysXClient::getTimeOut() const -{ - return m_data->m_timeOutInSeconds; -} - - -void PhysXClient::pushProfileTiming(const char* timingName) -{ - std::string** strPtr = m_data->m_profileTimingStringArray[timingName]; - std::string* str = 0; - if (strPtr) - { - str = *strPtr; - } - else - { - str = new std::string(timingName); - m_data->m_profileTimingStringArray.insert(timingName, str); - } - m_data->m_profileTimings.push_back(new CProfileSample(str->c_str())); -} - -void PhysXClient::popProfileTiming() -{ - if (m_data->m_profileTimings.size()) - { - CProfileSample* sample = m_data->m_profileTimings[m_data->m_profileTimings.size() - 1]; - m_data->m_profileTimings.pop_back(); - delete sample; - } -} - - - -#endif //BT_ENABLE_PHYSX diff --git a/examples/SharedMemory/physx/PhysXClient.h b/examples/SharedMemory/physx/PhysXClient.h deleted file mode 100644 index c5bd9f6ba..000000000 --- a/examples/SharedMemory/physx/PhysXClient.h +++ /dev/null @@ -1,135 +0,0 @@ -#ifndef PHYSX_CLIENT_H -#define PHYSX_CLIENT_H - -#include "../PhysicsClient.h" - -///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands -class PhysXClient : public PhysicsClient -{ -protected: - struct PhysXDirectInternalData* m_data; - - bool processDebugLines(const struct SharedMemoryCommand& orgCommand); - - bool processCamera(const struct SharedMemoryCommand& orgCommand); - - bool processContactPointData(const struct SharedMemoryCommand& orgCommand); - - bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand); - - bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand); - - void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); - - void processAddUserData(const struct SharedMemoryStatus& serverCmd); - - void postProcessStatus(const struct SharedMemoryStatus& serverCmd); - - void resetData(); - - void removeCachedBody(int bodyUniqueId); - -public: - PhysXClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership); - - virtual ~PhysXClient(); - - // return true if connection succesfull, can also check 'isConnected' - //it is OK to pass a null pointer for the gui helper - virtual bool connect(); - - ////todo: rename to 'disconnect' - virtual void disconnectSharedMemory(); - - virtual bool isConnected() const; - - // return non-null if there is a status, nullptr otherwise - virtual const SharedMemoryStatus* processServerStatus(); - - virtual SharedMemoryCommand* getAvailableSharedMemoryCommand(); - - virtual bool canSubmitCommand() const; - - virtual bool submitClientCommand(const struct SharedMemoryCommand& command); - - virtual int getNumBodies() const; - - virtual int getBodyUniqueId(int serialIndex) const; - - virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const; - - virtual int getNumJoints(int bodyIndex) const; - - virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; - - virtual int getNumUserConstraints() const; - - virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const; - - virtual int getUserConstraintId(int serialIndex) const; - - ///todo: move this out of the - virtual void setSharedMemoryKey(int key); - - void uploadBulletFileToSharedMemory(const char* data, int len); - - virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays); - - virtual int getNumDebugLines() const; - - virtual const float* getDebugLinesFrom() const; - virtual const float* getDebugLinesTo() const; - virtual const float* getDebugLinesColor() const; - - virtual void getCachedCameraImage(b3CameraImageData* cameraData); - - virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); - - virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects); - - virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); - - virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo); - - virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); - - virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); - - virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData); - - 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(); - virtual void debugDraw(int debugDrawMode); - - virtual void setTimeOut(double timeOutInSeconds); - virtual double getTimeOut() const; - - virtual bool getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const - { - return false; - } - virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const - { - return -1; - } - virtual int getNumUserData(int bodyUniqueId) const - { - return 0; - } - virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const - { - *keyOut = 0; - *userDataIdOut = -1; - return; - } - - virtual void pushProfileTiming(const char* timingName); - virtual void popProfileTiming(); -}; - -#endif //PHYSX_CLIENT_H diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index f0c26eac5..1832dd205 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -223,12 +223,10 @@ if not _OPTIONS["no-enet"] then "../../examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp", "../../examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h", "../../examples/SharedMemory/physx/PhysXC_API.cpp", - "../../examples/SharedMemory/physx/PhysXClient.cpp", "../../examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp", "../../examples/SharedMemory/physx/PhysXUrdfImporter.cpp", "../../examples/SharedMemory/physx/URDF2PhysX.cpp", "../../examples/SharedMemory/physx/PhysXC_API.h", - "../../examples/SharedMemory/physx/PhysXClient.h", "../../examples/SharedMemory/physx/PhysXServerCommandProcessor.h", "../../examples/SharedMemory/physx/PhysXUrdfImporter.h", "../../examples/SharedMemory/physx/URDF2PhysX.h", diff --git a/setup.py b/setup.py index fe39a57a3..bb20a20e7 100644 --- a/setup.py +++ b/setup.py @@ -142,7 +142,6 @@ else: sources = ["examples/pybullet/pybullet.c"]\ +["examples/SharedMemory/physx/PhysXC_API.cpp"]\ -+["examples/SharedMemory/physx/PhysXClient.cpp"]\ +["examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp"]\ +["examples/SharedMemory/physx/PhysXUrdfImporter.cpp"]\ +["examples/SharedMemory/physx/URDF2PhysX.cpp"]\