diff --git a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h index 53331e147..6e8dbed9e 100644 --- a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h +++ b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h @@ -5,7 +5,7 @@ #include //fopen #include "Bullet3Common/b3AlignedObjectArray.h" #include "../../CommonInterfaces/CommonFileIOInterface.h" - +#include //memcpy struct MySTLTriangle { float normal[3]; diff --git a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h index 7e01cffc7..02aa42534 100644 --- a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h +++ b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h @@ -95,6 +95,13 @@ struct UrdfRenderingInterface virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]) {} virtual void setProjectiveTexture(bool useProjectiveTexture) {} + + + virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const + { + return false; + } + }; #endif //LINK_VISUAL_SHAPES_CONVERTER_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/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index e864508b4..ce4c49267 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -658,6 +658,7 @@ public: } GUIHelperInterface* m_childGuiHelper; + btHashMap m_cachedTextureIds; int m_uidGenerator; const unsigned char* m_texels; int m_textureWidth; @@ -856,6 +857,11 @@ public: virtual int registerTexture(const unsigned char* texels, int width, int height) { + int* cachedTexture = m_cachedTextureIds[texels]; + if (cachedTexture) + { + return *cachedTexture; + } m_texels = texels; m_textureWidth = width; m_textureHeight = height; @@ -864,7 +870,7 @@ public: m_cs->setSharedParam(1, eGUIHelperRegisterTexture); workerThreadWait(); - + m_cachedTextureIds.insert(texels, m_textureId); return m_textureId; } virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId) @@ -918,6 +924,7 @@ public: virtual void removeAllGraphicsInstances() { + m_cachedTextureIds.clear(); m_cs->lock(); m_cs->setSharedParam(1, eGUIHelperRemoveAllGraphicsInstances); workerThreadWait(); diff --git a/examples/SharedMemory/physx/PhysXC_API.cpp b/examples/SharedMemory/physx/PhysXC_API.cpp index bd32027cf..0fb64afaf 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() + +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX(int argc, char* argv[]) { - PhysXServerCommandProcessor* sdk = new PhysXServerCommandProcessor; + PhysXServerCommandProcessor* sdk = new PhysXServerCommandProcessor(argc,argv); - 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/PhysXC_API.h b/examples/SharedMemory/physx/PhysXC_API.h index 54ad3e1a6..42d117fb9 100644 --- a/examples/SharedMemory/physx/PhysXC_API.h +++ b/examples/SharedMemory/physx/PhysXC_API.h @@ -10,7 +10,7 @@ extern "C" { #endif - B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX(); + B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX(int argc, char* argv[]); #ifdef __cplusplus } 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/SharedMemory/physx/PhysXServerCommandProcessor.cpp b/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp index c3a287f0d..d5aa86666 100644 --- a/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp +++ b/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp @@ -9,6 +9,7 @@ #include "Bullet3Common/b3AlignedObjectArray.h" #include "LinearMath/btMinMax.h" #include "Bullet3Common/b3FileUtils.h" +#include "Bullet3Common/b3CommandLineArgs.h" #include "../../Utils/b3ResourcePath.h" #include "Bullet3Common/b3ResizablePool.h" #include "PxPhysicsAPI.h" @@ -20,6 +21,7 @@ #include "URDF2PhysX.h" #include "../b3PluginManager.h" #include "PxRigidActorExt.h" +#include "LinearMath/btThreads.h" #define STATIC_EGLRENDERER_PLUGIN #ifdef STATIC_EGLRENDERER_PLUGIN @@ -48,6 +50,27 @@ public: } }; +class CustomProfilerCallback : public physx::PxProfilerCallback +{ +public: + virtual ~CustomProfilerCallback() {} + + virtual void* zoneStart(const char* eventName, bool detached, uint64_t contextId) + { + b3EnterProfileZone(eventName); + return 0; + } + + virtual void zoneEnd(void* profilerData, const char* eventName, bool detached, uint64_t contextId) + { + b3LeaveProfileZone(); + } + +}; + +static CustomProfilerCallback gCustomProfilerCallback; + + struct InternalPhysXBodyData { physx::PxArticulationReducedCoordinate* mArticulation; @@ -76,13 +99,118 @@ struct InternalPhysXBodyData typedef b3PoolBodyHandle InternalPhysXBodyHandle; -struct PhysXServerCommandProcessorInternalData + +struct PhysXServerCommandProcessorInternalData : public physx::PxSimulationEventCallback, public physx::PxContactModifyCallback { bool m_isConnected; bool m_verboseOutput; double m_physicsDeltaTime; int m_numSimulationSubSteps; + btSpinMutex m_taskLock; + btAlignedObjectArray m_contactPoints; + + + + void onContactModify(physx::PxContactModifyPair* const pairs, physx::PxU32 count) + { + for (physx::PxU32 i = 0; i contacts; + for (physx::PxU32 i = 0; i < nbPairs; i++) + { + physx::PxU32 contactCount = pairs[i].contactCount; + if (contactCount) + { + contacts.resize(contactCount); + pairs[i].extractContacts(&contacts[0], contactCount); + for (physx::PxU32 j = 0; j < contactCount; j++) + { + const physx::PxContactPairPoint& contact = contacts[i]; + b3ContactPointData srcPt; + MyPhysXUserData* udA = (MyPhysXUserData*)pairHeader.actors[0]->userData; + MyPhysXUserData* udB = (MyPhysXUserData*)pairHeader.actors[1]->userData; + srcPt.m_bodyUniqueIdA = udA->m_bodyUniqueId; + srcPt.m_linkIndexA = udA->m_linkIndex; + srcPt.m_bodyUniqueIdB = udB->m_bodyUniqueId; + srcPt.m_linkIndexB = udB->m_linkIndex; + srcPt.m_positionOnAInWS[0] = contact.position.x + contact.separation*contact.normal.x; + srcPt.m_positionOnAInWS[1] = contact.position.y + contact.separation*contact.normal.y; + srcPt.m_positionOnAInWS[2] = contact.position.z + contact.separation*contact.normal.z; + srcPt.m_positionOnBInWS[0] = contact.position.x - contact.separation*contact.normal.x; + srcPt.m_positionOnBInWS[1] = contact.position.y - contact.separation*contact.normal.y; + srcPt.m_positionOnBInWS[2] = contact.position.z - contact.separation*contact.normal.z; + srcPt.m_contactNormalOnBInWS[0] = contact.normal.x; + srcPt.m_contactNormalOnBInWS[1] = contact.normal.y; + srcPt.m_contactNormalOnBInWS[2] = contact.normal.z; + srcPt.m_contactDistance = contact.separation; + srcPt.m_contactFlags = 0; + srcPt.m_linearFrictionDirection1[0] = 0; + srcPt.m_linearFrictionDirection1[1] = 0; + srcPt.m_linearFrictionDirection1[2] = 0; + srcPt.m_linearFrictionDirection2[0] = 0; + srcPt.m_linearFrictionDirection2[1] = 0; + srcPt.m_linearFrictionDirection2[2] = 0; + + srcPt.m_linearFrictionForce2 = 0; + + srcPt.m_normalForce = contact.impulse.dot(contact.normal); + //compute friction direction from impulse projected in contact plane using contact normal. + physx::PxVec3 fric = contact.impulse - contact.normal*srcPt.m_normalForce; + double fricForce = fric.normalizeSafe(); + if (fricForce) + { + srcPt.m_linearFrictionDirection1[0] = fric.x; + srcPt.m_linearFrictionDirection1[1] = fric.y; + srcPt.m_linearFrictionDirection1[2] = fric.z; + srcPt.m_linearFrictionForce1 = fricForce; + } + m_contactPoints.push_back(srcPt); + // std::cout << "Contact: bw " << pairHeader.actors[0]->getName() << " and " << pairHeader.actors[1]->getName() << " | " << contacts[j].position.x << "," << contacts[j].position.y << "," + // << contacts[j].position.z << std::endl; + } + } + } + m_taskLock.unlock(); + } + + void onConstraintBreak(physx::PxConstraintInfo* constraints, physx::PxU32 count) + { + PX_UNUSED(constraints); + PX_UNUSED(count); + } + + void onWake(physx::PxActor** actors, physx::PxU32 count) + { + PX_UNUSED(actors); + PX_UNUSED(count); + } + + void onSleep(physx::PxActor** actors, physx::PxU32 count) + { + PX_UNUSED(actors); + PX_UNUSED(count); + } + + void onTrigger(physx::PxTriggerPair* pairs, physx::PxU32 count) + { + PX_UNUSED(pairs); + PX_UNUSED(count); + } + + void onAdvance(const physx::PxRigidBody* const*, const physx::PxTransform*, const physx::PxU32) + { + } b3PluginManager m_pluginManager; @@ -108,16 +236,23 @@ struct PhysXServerCommandProcessorInternalData int m_profileTimingLoggingUid; int m_stateLoggersUniqueId; std::string m_profileTimingFileName; + b3CommandLineArgs m_commandLineArgs; + int m_userDebugParametersUid; + btHashMap m_userDebugParameters; + btAlignedObjectArray m_graphicsIndexToSegmentationMask; - PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk) + PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk, int argc, char* argv[]) : m_isConnected(false), m_verboseOutput(false), m_physicsDeltaTime(1. / 240.), m_numSimulationSubSteps(0), m_pluginManager(sdk), m_profileTimingLoggingUid(-1), - m_stateLoggersUniqueId(1) + m_stateLoggersUniqueId(1), + m_commandLineArgs(argc,argv), + m_userDebugParametersUid(0) { + m_foundation = NULL; m_physics = NULL; m_cooking = NULL; @@ -138,9 +273,10 @@ struct PhysXServerCommandProcessorInternalData } }; -PhysXServerCommandProcessor::PhysXServerCommandProcessor() +PhysXServerCommandProcessor::PhysXServerCommandProcessor(int argc, char* argv[]) { - m_data = new PhysXServerCommandProcessorInternalData(this); + + m_data = new PhysXServerCommandProcessorInternalData(this, argc, argv); } PhysXServerCommandProcessor::~PhysXServerCommandProcessor() @@ -158,9 +294,10 @@ physx::PxFilterFlags MyPhysXFilter(physx::PxFilterObjectAttributes attributes0, PX_UNUSED(attributes1); PX_UNUSED(constantBlock); PX_UNUSED(constantBlockSize); - if (filterData0.word2 != 0 && filterData0.word2 == filterData1.word2) - return physx::PxFilterFlag::eKILL; - pairFlags |= physx::PxPairFlag::eCONTACT_DEFAULT; + // if (filterData0.word2 != 0 && filterData0.word2 == filterData1.word2) + // return physx::PxFilterFlag::eKILL; + pairFlags |= physx::PxPairFlag::eCONTACT_DEFAULT | physx::PxPairFlag::eNOTIFY_TOUCH_FOUND + | physx::PxPairFlag::eDETECT_DISCRETE_CONTACT | physx::PxPairFlag::eNOTIFY_CONTACT_POINTS | physx::PxPairFlag::eMODIFY_CONTACTS; return physx::PxFilterFlag::eDEFAULT; } @@ -178,21 +315,57 @@ bool PhysXServerCommandProcessor::connect() { m_data->m_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, m_data->m_allocator, m_data->m_errorCallback); + // This call should be performed after PVD is initialized, otherwise it will have no effect. + PxSetProfilerCallback(&gCustomProfilerCallback); + m_data->m_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxTolerancesScale(), true, 0); m_data->m_cooking = PxCreateCooking(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxCookingParams(physx::PxTolerancesScale())); - physx::PxU32 numCores = 1;// + + physx::PxU32 numCores = 1; + m_data->m_commandLineArgs.GetCmdLineArgument("numCores", numCores); + printf("PhysX numCores=%d\n", numCores); m_data->m_dispatcher = physx::PxDefaultCpuDispatcherCreate(numCores == 0 ? 0 : numCores - 1); physx::PxSceneDesc sceneDesc(m_data->m_physics->getTolerancesScale()); sceneDesc.gravity = physx::PxVec3(0.0f, -9.81f, 0.0f); - sceneDesc.solverType = physx::PxSolverType::eTGS; - //sceneDesc.solverType = physx::PxSolverType::ePGS; + + + sceneDesc.solverType = physx::PxSolverType::ePGS; + std::string solver; + m_data->m_commandLineArgs.GetCmdLineArgument("solver", solver); + + if (solver=="tgs") + { + sceneDesc.solverType = physx::PxSolverType::eTGS; + printf("PhysX using TGS\n"); + } + else + { + printf("PhysX using PGS\n"); + } + sceneDesc.cpuDispatcher = m_data->m_dispatcher; - //sceneDesc.filterShader = MyPhysXFilter; - sceneDesc.filterShader = physx::PxDefaultSimulationFilterShader; - + + //todo: add some boolean, to allow enable/disable of this contact filtering + bool enableContactCallback = false; + m_data->m_commandLineArgs.GetCmdLineArgument("enableContactCallback", enableContactCallback); + if (enableContactCallback) + { + sceneDesc.filterShader = MyPhysXFilter; + sceneDesc.simulationEventCallback = this->m_data; + sceneDesc.contactModifyCallback = this->m_data; + printf("PhysX enableContactCallback\n"); + } + else + { + sceneDesc.filterShader = physx::PxDefaultSimulationFilterShader; + } + + + + m_data->m_scene = m_data->m_physics->createScene(sceneDesc); m_data->m_material = m_data->m_physics->createMaterial(0.5f, 0.5f, 0.f); @@ -588,13 +761,14 @@ bool PhysXServerCommandProcessor::processSendDesiredStateCommand(const struct Sh //find the joint motors and apply the desired velocity and maximum force/torque { int dofIndex = 6; //skip the 3 linear + 3 angular degree of freedom entries of the base + int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base for (int link = 1; link < numLinks2; link++) { int dofs = physxLinks[link]->getInboundJointDof(); physx::PxReal stiffness = 10.f; physx::PxReal damping = 0.1f; physx::PxReal forceLimit = PX_MAX_F32; - int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base + if (dofs == 1) { @@ -614,33 +788,34 @@ bool PhysXServerCommandProcessor::processSendDesiredStateCommand(const struct Sh if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0) { desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; - } - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0) - { - kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; - } - physx::PxReal damping = kd; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KP) != 0) - { - kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[dofIndex]; - stiffness = kp; - } + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0) + { + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; + } + physx::PxReal damping = kd; - joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity); - - + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KP) != 0) + { + kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[dofIndex]; + stiffness = kp; + } - physx::PxReal forceLimit = 1000000.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) - { - forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]; + joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity); + + + + physx::PxReal forceLimit = 1000000.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) + { + forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]; + } + bool isAcceleration = false; + + joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, desiredPosition); + joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit, isAcceleration); } - bool isAcceleration = false; - joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit, isAcceleration); - - joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, desiredPosition); } dofIndex += dofs; @@ -1049,6 +1224,91 @@ bool PhysXServerCommandProcessor::processRequestPhysicsSimulationParametersComma return hasStatus; } +bool PhysXServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + SharedMemoryStatus& serverCmd = serverStatusOut; + int totalBytesPerContact = sizeof(b3ContactPointData); + int contactPointStorage = bufferSizeInBytes / totalBytesPerContact - 1; + + b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient; + + int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; + int numContactPointBatch = btMin(int(m_data->m_contactPoints.size()), contactPointStorage); + + int endContactPointIndex = startContactPointIndex + numContactPointBatch; + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0; + for (int i = startContactPointIndex; i < endContactPointIndex; i++) + { + const b3ContactPointData& srcPt = m_data->m_contactPoints[i]; + b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied]; + destPt = srcPt; + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++; + } + serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = startContactPointIndex; + serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = m_data->m_contactPoints.size() - startContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; + + + return true; +} + +bool PhysXServerCommandProcessor::processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + return false; +} + + +bool PhysXServerCommandProcessor::processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_SET_ADDITIONAL_SEARCH_PATH"); + b3ResourcePath::setAdditionalSearchPath(clientCmd.m_searchPathArgs.m_path); + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysXServerCommandProcessor::processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + ///dummy, so commands don't fail + bool hasStatus = true; + BT_PROFILE("CMD_USER_DEBUG_DRAW"); + SharedMemoryStatus& serverCmd = serverStatusOut; + int uid = 0; + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; + + if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER) + { + int uid = m_data->m_userDebugParametersUid++; + double value = clientCmd.m_userDebugDrawArgs.m_startValue; + m_data->m_userDebugParameters.insert(uid, value); + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags & USER_DEBUG_READ_PARAMETER) + { + double* valPtr = m_data->m_userDebugParameters[clientCmd.m_userDebugDrawArgs.m_itemUniqueId]; + if (valPtr) + { + serverCmd.m_userDebugDrawArgs.m_parameterValue = *valPtr; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED; + } + } + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL) + { + m_data->m_userDebugParameters.clear(); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) + { + m_data->m_userDebugParameters.remove(clientCmd.m_userDebugDrawArgs.m_itemUniqueId); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + return hasStatus; + +} bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { @@ -1057,7 +1317,7 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman int sz = sizeof(SharedMemoryStatus); int sz2 = sizeof(SharedMemoryCommand); - bool hasStatus = false; + bool hasStatus = true; serverStatusOut.m_type = CMD_INVALID_STATUS; serverStatusOut.m_numDataStreamBytes = 0; @@ -1111,15 +1371,6 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman break; } - default: - { - BT_PROFILE("CMD_UNKNOWN"); - printf("Unknown command encountered: %d", clientCmd.m_type); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; - hasStatus = true; - } - case CMD_LOAD_URDF: { hasStatus = processLoadURDFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); @@ -1160,6 +1411,35 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman break; } + case CMD_REQUEST_CONTACT_POINT_INFORMATION: + { + hasStatus = processRequestContactpointInformationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_CREATE_COLLISION_SHAPE: + { + hasStatus = processCreateCollisionShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SET_ADDITIONAL_SEARCH_PATH: + { + hasStatus = processSetAdditionalSearchPathCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_USER_DEBUG_DRAW: + { + hasStatus = processUserDebugDrawCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_CAMERA_IMAGE_DATA: + { + hasStatus = processRequestCameraImageCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } #if 0 case CMD_SET_VR_CAMERA_STATE: @@ -1195,11 +1475,6 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman break; } - case CMD_REQUEST_CAMERA_IMAGE_DATA: - { - hasStatus = processRequestCameraImageCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } case CMD_REQUEST_BODY_INFO: { @@ -1216,11 +1491,7 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman hasStatus = processLoadSDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); break; } - case CMD_CREATE_COLLISION_SHAPE: - { - hasStatus = processCreateCollisionShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + case CMD_CREATE_VISUAL_SHAPE: { hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); @@ -1231,11 +1502,7 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); break; } - case CMD_SET_ADDITIONAL_SEARCH_PATH: - { - hasStatus = processSetAdditionalSearchPathCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + case CMD_LOAD_MJCF: { @@ -1319,11 +1586,7 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); break; } - case CMD_REQUEST_CONTACT_POINT_INFORMATION: - { - hasStatus = processRequestContactpointInformationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + case CMD_CALCULATE_INVERSE_DYNAMICS: { hasStatus = processInverseDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); @@ -1345,45 +1608,45 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman break; } case CMD_REMOVE_BODY: - { - hasStatus = processRemoveBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processRemoveBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_USER_CONSTRAINT: - { - hasStatus = processCreateUserConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processCreateUserConstraintCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_CALCULATE_INVERSE_KINEMATICS: - { - hasStatus = processCalculateInverseKinematicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processCalculateInverseKinematicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_REQUEST_VISUAL_SHAPE_INFO: - { - hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processRequestVisualShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_REQUEST_COLLISION_SHAPE_INFO: { hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_UPDATE_VISUAL_SHAPE: - { - hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processUpdateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_CHANGE_TEXTURE: - { - hasStatus = processChangeTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processChangeTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_LOAD_TEXTURE: - { - hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processLoadTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_RESTORE_STATE: { hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); @@ -1397,47 +1660,291 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman } case CMD_LOAD_BULLET: - { - hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processLoadBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_SAVE_BULLET: - { - hasStatus = processSaveBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processSaveBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_LOAD_MJCF: - { - hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } - case CMD_USER_DEBUG_DRAW: - { - hasStatus = processUserDebugDrawCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } - + { + hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_USER_DATA: - { - hasStatus = processRequestUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processRequestUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_ADD_USER_DATA: - { - hasStatus = processAddUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processAddUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_REMOVE_USER_DATA: - { - hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processRemoveUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } #endif + + default: + { + BT_PROFILE("CMD_UNKNOWN"); + printf("Unknown command encountered: %d", clientCmd.m_type); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; + hasStatus = true; + } }; return hasStatus; } + + +bool PhysXServerCommandProcessor::processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA"); + int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex; + int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth; + int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight; + int numPixelsCopied = 0; + + int oldWidth; + int oldHeight; + m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(oldWidth, oldHeight); + + serverStatusOut.m_type = CMD_CAMERA_IMAGE_FAILED; + + if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0) && + (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT) != 0) + { + if (m_data->m_pluginManager.getRenderInterface()) + { + + m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, + clientCmd.m_requestPixelDataArguments.m_pixelHeight); + } + } + int flags = 0; + if (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_FLAGS) + { + flags = clientCmd.m_requestPixelDataArguments.m_flags; + } + if (m_data->m_pluginManager.getRenderInterface()) + { + m_data->m_pluginManager.getRenderInterface()->setFlags(flags); + } + + int numTotalPixels = width * height; + int numRemainingPixels = numTotalPixels - startPixelIndex; + + if (numRemainingPixels > 0) + { + int totalBytesPerPixel = 4 + 4 + 4; //4 for rgb, 4 for depth, 4 for segmentation mask + int maxNumPixels = bufferSizeInBytes / totalBytesPerPixel - 1; + unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient; + int numRequestedPixels = btMin(maxNumPixels, numRemainingPixels); + + float* depthBuffer = (float*)(bufferServerToClient + numRequestedPixels * 4); + int* segmentationMaskBuffer = (int*)(bufferServerToClient + numRequestedPixels * 8); + + serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel; + float viewMat[16]; + float projMat[16]; + float projTextureViewMat[16]; + float projTextureProjMat[16]; + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) == 0) + { + b3OpenGLVisualizerCameraInfo tmpCamResult; + bool result = m_data->m_pluginManager.getRenderInterface()->getCameraInfo( + &tmpCamResult.m_width, + &tmpCamResult.m_height, + tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix, + tmpCamResult.m_camUp, + tmpCamResult.m_camForward, + tmpCamResult.m_horizontal, + tmpCamResult.m_vertical, + &tmpCamResult.m_yaw, + &tmpCamResult.m_pitch, + &tmpCamResult.m_dist, + tmpCamResult.m_target); + if (result) + { + for (int i = 0; i < 16; i++) + { + viewMat[i] = tmpCamResult.m_viewMatrix[i]; + projMat[i] = tmpCamResult.m_projectionMatrix[i]; + } + } + else + { + //failed + m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight); + return hasStatus; + } + } + else + { + for (int i = 0; i < 16; i++) + { + viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i]; + projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i]; + } + } + + { + if (m_data->m_pluginManager.getRenderInterface()) + { + if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0) + { + // printf("-------------------------------\nRendering\n"); + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow != 0)); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff); + } + + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) != 0) + { + m_data->m_pluginManager.getRenderInterface()->render( + clientCmd.m_requestPixelDataArguments.m_viewMatrix, + clientCmd.m_requestPixelDataArguments.m_projectionMatrix); + } + else + { + b3OpenGLVisualizerCameraInfo tmpCamResult; + bool result = m_data->m_pluginManager.getRenderInterface()->getCameraInfo( + &tmpCamResult.m_width, + &tmpCamResult.m_height, + tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix, + tmpCamResult.m_camUp, + tmpCamResult.m_camForward, + tmpCamResult.m_horizontal, + tmpCamResult.m_vertical, + &tmpCamResult.m_yaw, + &tmpCamResult.m_pitch, + &tmpCamResult.m_dist, + tmpCamResult.m_target); + if (result) + { + m_data->m_pluginManager.getRenderInterface()->render(tmpCamResult.m_viewMatrix, tmpCamResult.m_projectionMatrix); + } + else + { + m_data->m_pluginManager.getRenderInterface()->render(); + } + } + } + } + + if (m_data->m_pluginManager.getRenderInterface()) + { + if ((flags & ER_USE_PROJECTIVE_TEXTURE) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(true); + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES) != 0) + { + for (int i = 0; i < 16; i++) + { + projTextureViewMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i]; + projTextureProjMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i]; + } + } + else // If no specified matrices for projective texture, then use the camera matrices. + { + for (int i = 0; i < 16; i++) + { + projTextureViewMat[i] = viewMat[i]; + projTextureProjMat[i] = projMat[i]; + } + } + m_data->m_pluginManager.getRenderInterface()->setProjectiveTextureMatrices(projTextureViewMat, projTextureProjMat); + } + else + { + m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false); + } + + if ((flags & ER_NO_SEGMENTATION_MASK) != 0) + { + segmentationMaskBuffer = 0; + } + + m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels, + depthBuffer, numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex, &width, &height, &numPixelsCopied); + m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false); + } + + #if 0 + m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix, + clientCmd.m_requestPixelDataArguments.m_projectionMatrix, pixelRGBA, numRequestedPixels, + depthBuffer, numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex, width, height, &numPixelsCopied); + #endif + } + + //each pixel takes 4 RGBA values and 1 float = 8 bytes + } + else + { + } + + m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight); + serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED; + + serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied; + serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied; + serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex; + serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width; + serverStatusOut.m_sendPixelDataArguments.m_imageHeight = height; + return hasStatus; + +} + bool PhysXServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; @@ -1767,8 +2274,13 @@ bool PhysXServerCommandProcessor::processForwardDynamicsCommand(const struct Sha int numArt = m_data->m_scene->getNbArticulations(); + { + B3_PROFILE("clear Contacts"); + m_data->m_contactPoints.clear(); + } { B3_PROFILE("PhysX_simulate_fetchResults"); + m_data->m_scene->simulate(m_data->m_physicsDeltaTime); m_data->m_scene->fetchResults(true); } diff --git a/examples/SharedMemory/physx/PhysXServerCommandProcessor.h b/examples/SharedMemory/physx/PhysXServerCommandProcessor.h index 5cf8fa058..7566268e2 100644 --- a/examples/SharedMemory/physx/PhysXServerCommandProcessor.h +++ b/examples/SharedMemory/physx/PhysXServerCommandProcessor.h @@ -20,14 +20,19 @@ class PhysXServerCommandProcessor : public PhysicsCommandProcessorInterface bool processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); - + bool processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); void resetSimulation(); bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + public: - PhysXServerCommandProcessor(); + PhysXServerCommandProcessor(int argc, char* argv[]); virtual ~PhysXServerCommandProcessor(); diff --git a/examples/SharedMemory/physx/PhysXUserData.h b/examples/SharedMemory/physx/PhysXUserData.h index 8f4642a81..b4149ee0c 100644 --- a/examples/SharedMemory/physx/PhysXUserData.h +++ b/examples/SharedMemory/physx/PhysXUserData.h @@ -4,5 +4,7 @@ struct MyPhysXUserData { int m_graphicsUniqueId; + int m_bodyUniqueId; + int m_linkIndex; }; #endif //PHYSX_USER_DATA_H \ No newline at end of file diff --git a/examples/SharedMemory/physx/URDF2PhysX.cpp b/examples/SharedMemory/physx/URDF2PhysX.cpp index b810fb7d5..2a5a19a87 100644 --- a/examples/SharedMemory/physx/URDF2PhysX.cpp +++ b/examples/SharedMemory/physx/URDF2PhysX.cpp @@ -281,11 +281,21 @@ int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedDat { btVector3 planeNormal = col.m_geometry.m_planeNormal; btScalar planeConstant = 0; //not available? - //PxPlane(1, 0, 0, 0). + + btVector3 planeRefAxis(1, 0, 0); + btQuaternion diffQuat = shortestArcQuat(planeRefAxis, planeNormal); shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxPlaneGeometry(), *material); - //todo: compensate for plane axis - //physx::PxTransform localPose = tr*physx::PxTransform - //shape->setLocalPose(localPose); + + btTransform diffTr; + diffTr.setIdentity(); + diffTr.setRotation(diffQuat); + btTransform localTrans = localInertiaFrame.inverse()*childTrans*diffTr; + physx::PxTransform tr; + tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]); + tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]); + + physx::PxTransform localPose = tr; + shape->setLocalPose(localPose); numShapes++; break; } @@ -294,6 +304,13 @@ int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedDat btScalar radius = collision->m_geometry.m_capsuleRadius; btScalar height = collision->m_geometry.m_capsuleHeight; + //static PxShape* createExclusiveShape(PxRigidActor& actor, const PxGeometry& geometry, PxMaterial*const* materials, PxU16 materialCount, + // PxShapeFlags shapeFlags = PxShapeFlag::eVISUALIZATION | PxShapeFlag::eSCENE_QUERY_SHAPE | PxShapeFlag::eSIMULATION_SHAPE) + //{ + physx::PxShapeFlags shapeFlags = physx::PxShapeFlag::eVISUALIZATION | physx::PxShapeFlag::eSCENE_QUERY_SHAPE | physx::PxShapeFlag::eSIMULATION_SHAPE; + + //shape = PxGetPhysics().createShape(physx::PxCapsuleGeometry(radius, 0.5*height), &material, 1, false, shapeFlags); + shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxCapsuleGeometry(radius, 0.5*height), *material); btTransform childTrans = col.m_linkLocalFrame; @@ -691,6 +708,7 @@ btTransform ConvertURDF2PhysXInternal( //Now create the slider and fixed joints... + //cache.m_articulation->setSolverIterationCounts(4);//todo: API? cache.m_articulation->setSolverIterationCounts(32);//todo: API? cache.m_jointTypes.push_back(physx::PxArticulationJointType::eUNDEFINED); @@ -796,6 +814,8 @@ btTransform ConvertURDF2PhysXInternal( //todo: mem leaks MyPhysXUserData* userData = new MyPhysXUserData(); userData->m_graphicsUniqueId = graphicsIndex; + userData->m_bodyUniqueId = u2b.getBodyUniqueId(); + userData->m_linkIndex = mbLinkIndex; linkPtr->userData = userData; } @@ -805,8 +825,10 @@ btTransform ConvertURDF2PhysXInternal( convertLinkPhysXShapes(u2b, cache, creation, urdfLinkIndex, pathPrefix, localInertialFrame, cache.m_articulation, mbLinkIndex, linkPtr); - physx::PxRigidBodyExt::updateMassAndInertia(*rbLinkPtr, mass); - + if (rbLinkPtr && mass) + { + physx::PxRigidBodyExt::updateMassAndInertia(*rbLinkPtr, mass); + } //base->setMass(massOut); //base->setMassSpaceInertiaTensor(diagTensor); @@ -1003,6 +1025,7 @@ physx::PxBase* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* phy if (cache.m_rigidStatic) { + scene->addActor(*cache.m_rigidStatic); return cache.m_rigidStatic; } diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp index 9f4943e4d..f6eb2ae05 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp @@ -72,7 +72,7 @@ struct MyTexture3 struct EGLRendererObjectArray { - btAlignedObjectArray m_renderObjects; + btAlignedObjectArray m_graphicsInstanceIds; int m_objectUniqueId; int m_linkIndex; @@ -92,6 +92,33 @@ struct EGLRendererObjectArray #define START_WIDTH 1024 #define START_HEIGHT 768 +struct btHashVisual +{ + UrdfShape m_vis; + btTransform m_tr; + + int getHash() const + { + if (m_vis.m_geometry.m_meshFileName.length()) + { + btHashString s = m_vis.m_geometry.m_meshFileName.c_str(); + return s.getHash(); + } + return 0; + } + bool equals(const btHashVisual& other) const + { + if ((m_vis.m_geometry.m_type == URDF_GEOM_MESH) && + (other.m_vis.m_geometry.m_type == URDF_GEOM_MESH)) + { + bool sameTr = m_tr == other.m_tr; + bool sameVis = m_vis.m_geometry.m_meshFileName == other.m_vis.m_geometry.m_meshFileName; + bool sameLocalFrame = m_vis.m_linkLocalFrame == other.m_vis.m_linkLocalFrame; + return sameTr&&sameVis&&sameLocalFrame; + } + return false; + } +}; struct EGLRendererVisualShapeConverterInternalData { @@ -105,6 +132,8 @@ struct EGLRendererVisualShapeConverterInternalData btAlignedObjectArray m_graphicsIndexToSegmentationMask; btHashMap m_swRenderInstances; + btHashMap m_cachedTextureIds; + btHashMap m_cachedVisualShapes; btAlignedObjectArray m_visualShapes; @@ -942,7 +971,7 @@ int EGLRendererVisualShapeConverter::convertVisualShapes( colorIndex &= 3; btVector4 color; color = sColors[colorIndex]; - float rgbaColor[4] = {(float)color[0], (float)color[1], (float)color[2], (float)color[3]}; + float rgbaColor[4] = { (float)color[0], (float)color[1], (float)color[2], (float)color[3] }; //if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE) //{ // color.setValue(1,1,1,1); @@ -1012,64 +1041,91 @@ int EGLRendererVisualShapeConverter::convertVisualShapes( visualShape.m_rgbaColor[1] = rgbaColor[1]; visualShape.m_rgbaColor[2] = rgbaColor[2]; visualShape.m_rgbaColor[3] = rgbaColor[3]; + int shapeIndex = -1; + btHashVisual tmp; { B3_PROFILE("convertURDFToVisualShape2"); - convertURDFToVisualShape2(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags); + + + + btTransform tr = localInertiaFrame.inverse() * childTrans; + tmp.m_vis = *vis; + tmp.m_tr = tr; + + int* bla = m_data->m_cachedVisualShapes[tmp]; + if (bla) + { + shapeIndex = *bla; + } + else + { + convertURDFToVisualShape2(vis, pathPrefix, tr, vertices, indices, textures, visualShape, fileIO, m_data->m_flags); + } } m_data->m_visualShapes.push_back(visualShape); - if (vertices.size() && indices.size()) + int textureIndex = -1; + if (shapeIndex < 0) { - TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer, m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex); - unsigned char* textureImage1 = 0; - int textureWidth = 0; - int textureHeight = 0; - bool isCached = false; - int textureIndex = -1; - - if (textures.size()) + if (vertices.size() && indices.size()) { - textureImage1 = textures[0].textureData1; - textureWidth = textures[0].m_width; - textureHeight = textures[0].m_height; - isCached = textures[0].m_isCached; - textureIndex = m_data->m_instancingRenderer->registerTexture(textureImage1, textureWidth, textureHeight); - } + unsigned char* textureImage1 = 0; + int textureWidth = 0; + int textureHeight = 0; + bool isCached = false; - { - B3_PROFILE("registerMeshShape"); - tinyObj->registerMeshShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), rgbaColor, - textureImage1, textureWidth, textureHeight); - } - visuals->m_renderObjects.push_back(tinyObj); - - { - B3_PROFILE("m_instancingRenderer register"); - - // register mesh to m_instancingRenderer too. - - int shapeIndex = m_data->m_instancingRenderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); - double scaling[3] = {1, 1, 1}; - int graphicsIndex = m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0], scaling); - - int segmentationMask = bodyUniqueId + ((linkIndex + 1) << 24); + if (textures.size()) { - if (graphicsIndex >= 0) + textureImage1 = textures[0].textureData1; + textureWidth = textures[0].m_width; + textureHeight = textures[0].m_height; + isCached = textures[0].m_isCached; + int* bla = m_data->m_cachedTextureIds[textureImage1]; + if (bla) { - visuals->m_graphicsInstanceIds.push_back(graphicsIndex); - - if (m_data->m_graphicsIndexToSegmentationMask.size() < (graphicsIndex + 1)) - { - m_data->m_graphicsIndexToSegmentationMask.resize(graphicsIndex + 1); - } - m_data->m_graphicsIndexToSegmentationMask[graphicsIndex] = segmentationMask; + textureIndex = *bla; + } + else + { + textureIndex = m_data->m_instancingRenderer->registerTexture(textureImage1, textureWidth, textureHeight); + m_data->m_cachedTextureIds.insert(textureImage1, textureIndex); } } - - m_data->m_instancingRenderer->writeTransforms(); } } + + + { + B3_PROFILE("m_instancingRenderer register"); + + // register mesh to m_instancingRenderer too. + + if (shapeIndex < 0) + { + shapeIndex = m_data->m_instancingRenderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); + m_data->m_cachedVisualShapes.insert(tmp, shapeIndex); + } + double scaling[3] = { 1, 1, 1 }; + int graphicsIndex = m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0], scaling); + + int segmentationMask = bodyUniqueId + ((linkIndex + 1) << 24); + { + if (graphicsIndex >= 0) + { + visuals->m_graphicsInstanceIds.push_back(graphicsIndex); + + if (m_data->m_graphicsIndexToSegmentationMask.size() < (graphicsIndex + 1)) + { + m_data->m_graphicsIndexToSegmentationMask.resize(graphicsIndex + 1); + } + m_data->m_graphicsIndexToSegmentationMask[graphicsIndex] = segmentationMask; + } + } + + m_data->m_instancingRenderer->writeTransforms(); + } + for (int i = 0; i < textures.size(); i++) { if (!textures[i].m_isCached) @@ -1078,6 +1134,7 @@ int EGLRendererVisualShapeConverter::convertVisualShapes( } } } + } return orgGraphicsUniqueId; } @@ -1163,13 +1220,7 @@ void EGLRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int link EGLRendererObjectArray* visuals = *ptrptr; if ((bodyUniqueId == visuals->m_objectUniqueId) && (linkIndex == visuals->m_linkIndex)) { - for (int q = 0; q < visuals->m_renderObjects.size(); q++) - { - if (shapeIndex < 0 || q == shapeIndex) - { - visuals->m_renderObjects[q]->m_model->setColorRGBA(rgba); - } - } + } } } @@ -1225,6 +1276,8 @@ void EGLRendererVisualShapeConverter::render(const float viewMat[16], const floa render(); + m_data->m_camera.disableVRCamera(); + //cout<m_renderObjects.size(); o++) + for (int i = 0; i < ptr->m_graphicsInstanceIds.size(); i++) { - for (int i = 0; i < ptr->m_graphicsInstanceIds.size(); i++) - { - m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceIds[i]); - } - - delete ptr->m_renderObjects[o]; + m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceIds[i]); } + } delete ptr; m_data->m_swRenderInstances.remove(collisionObjectUniqueId); @@ -1520,6 +1569,8 @@ void EGLRendererVisualShapeConverter::removeVisualShape(int collisionObjectUniqu void EGLRendererVisualShapeConverter::resetAll() { + m_data->m_cachedTextureIds.clear(); + for (int i = 0; i < m_data->m_swRenderInstances.size(); i++) { EGLRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i); @@ -1528,10 +1579,7 @@ void EGLRendererVisualShapeConverter::resetAll() EGLRendererObjectArray* ptr = *ptrptr; if (ptr) { - for (int o = 0; o < ptr->m_renderObjects.size(); o++) - { - delete ptr->m_renderObjects[o]; - } + } delete ptr; } @@ -1556,25 +1604,7 @@ void EGLRendererVisualShapeConverter::changeShapeTexture(int objectUniqueId, int btAssert(textureUniqueId < m_data->m_textures.size()); if (textureUniqueId >= 0 && textureUniqueId < m_data->m_textures.size()) { - for (int n = 0; n < m_data->m_swRenderInstances.size(); n++) - { - EGLRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(n); - if (0 == visualArrayPtr) - continue; //can this ever happen? - EGLRendererObjectArray* visualArray = *visualArrayPtr; - - if (visualArray->m_objectUniqueId == objectUniqueId && visualArray->m_linkIndex == jointIndex) - { - for (int v = 0; v < visualArray->m_renderObjects.size(); v++) - { - TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v]; - if ((shapeIndex < 0) || (shapeIndex == v)) - { - renderObj->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData1, m_data->m_textures[textureUniqueId].m_width, m_data->m_textures[textureUniqueId].m_height); - } - } - } - } + } } @@ -1651,3 +1681,59 @@ void EGLRendererVisualShapeConverter::syncTransform(int collisionObjectUniqueId, } } } + +bool EGLRendererVisualShapeConverter::getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const +{ + if (m_data->m_instancingRenderer && m_data->m_instancingRenderer->getActiveCamera()) + { + *width = m_data->m_window->getWidth() * m_data->m_window->getRetinaScale(); + *height = m_data->m_window->getHeight() * m_data->m_window->getRetinaScale(); + m_data->m_instancingRenderer->getActiveCamera()->getCameraViewMatrix(viewMatrix); + m_data->m_instancingRenderer->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); + m_data->m_instancingRenderer->getActiveCamera()->getCameraUpVector(camUp); + m_data->m_instancingRenderer->getActiveCamera()->getCameraForwardVector(camForward); + + float top = 1.f; + float bottom = -1.f; + float tanFov = (top - bottom) * 0.5f / 1; + float fov = btScalar(2.0) * btAtan(tanFov); + btVector3 camPos, camTarget; + m_data->m_instancingRenderer->getActiveCamera()->getCameraPosition(camPos); + m_data->m_instancingRenderer->getActiveCamera()->getCameraTargetPosition(camTarget); + btVector3 rayFrom = camPos; + btVector3 rayForward = (camTarget - camPos); + rayForward.normalize(); + float farPlane = 10000.f; + rayForward *= farPlane; + + btVector3 rightOffset; + btVector3 cameraUp = btVector3(camUp[0], camUp[1], camUp[2]); + btVector3 vertical = cameraUp; + btVector3 hori; + hori = rayForward.cross(vertical); + hori.normalize(); + vertical = hori.cross(rayForward); + vertical.normalize(); + float tanfov = tanf(0.5f * fov); + hori *= 2.f * farPlane * tanfov; + vertical *= 2.f * farPlane * tanfov; + btScalar aspect = float(*width) / float(*height); + hori *= aspect; + //compute 'hor' and 'vert' vectors, useful to generate raytracer rays + hor[0] = hori[0] * m_data->m_window->getRetinaScale(); + hor[1] = hori[1] * m_data->m_window->getRetinaScale(); + hor[2] = hori[2] * m_data->m_window->getRetinaScale(); + vert[0] = vertical[0] * m_data->m_window->getRetinaScale(); + vert[1] = vertical[1] * m_data->m_window->getRetinaScale(); + vert[2] = vertical[2] * m_data->m_window->getRetinaScale(); + + *yaw = m_data->m_instancingRenderer->getActiveCamera()->getCameraYaw(); + *pitch = m_data->m_instancingRenderer->getActiveCamera()->getCameraPitch(); + *camDist = m_data->m_instancingRenderer->getActiveCamera()->getCameraDistance(); + cameraTarget[0] = camTarget[0]; + cameraTarget[1] = camTarget[1]; + cameraTarget[2] = camTarget[2]; + return true; + } + return false; +} diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h index 5fe8a5f08..b0cfc36a7 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h @@ -59,6 +59,9 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface virtual void mouseMoveCallback(float x, float y); virtual void mouseButtonCallback(int button, int state, float x, float y); + virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const; + + }; #endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H diff --git a/examples/pybullet/examples/otherPhysicsEngine.py b/examples/pybullet/examples/otherPhysicsEngine.py index 3399cff4f..93f9fbe44 100644 --- a/examples/pybullet/examples/otherPhysicsEngine.py +++ b/examples/pybullet/examples/otherPhysicsEngine.py @@ -1,21 +1,64 @@ import pybullet as p +import pybullet_data as pd import time import math usePhysX = True +useMaximalCoordinates = True if usePhysX: - p.connect(p.PhysX) + p.connect(p.PhysX,options="--numCores=8 --solver=pgs") p.loadPlugin("eglRendererPlugin") else: p.connect(p.GUI) -p.loadURDF("plane.urdf") +p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024) +p.setPhysicsEngineParameter(contactBreakingThreshold=0.01) -for i in range (10): - sphere = p.loadURDF("marble_cube.urdf",[0,-1,1+i*1], useMaximalCoordinates=False) -p.changeDynamics(sphere ,-1, mass=1000) +p.setAdditionalSearchPath(pd.getDataPath()) +#Always make ground plane maximal coordinates, to avoid performance drop in PhysX +#See https://github.com/NVIDIAGameWorks/PhysX/issues/71 -door = p.loadURDF("door.urdf",[0,0,1]) +p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates) +p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) +logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json") +jran = 50 +iran = 100 + +num=64 +radius=0.1 +numDominoes=0 + +for i in range (int(num*50)): + num=(radius*2*math.pi)/0.08 + radius += 0.05/float(num) + orn = p.getQuaternionFromEuler([0,0,0.5*math.pi+math.pi*2*i/float(num)]) + pos = [radius*math.cos(2*math.pi*(i/float(num))),radius*math.sin(2*math.pi*(i/float(num))), 0.03] + sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates) + numDominoes+=1 + +pos=[pos[0],pos[1],pos[2]+0.3] +orn = p.getQuaternionFromEuler([0,0,-math.pi/4.]) +sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates) + +print("numDominoes=",numDominoes) + + +#for j in range (20): +# for i in range (100): +# if (i<99): +# sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates) +# else: +# orn = p.getQuaternionFromEuler([0,-3.14*0.24,0]) +# sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates) + + +print("loaded!") + + +#p.changeDynamics(sphere ,-1, mass=1000) + +door = p.loadURDF("door.urdf",[0,0,-11]) p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1) print("numJoints = ", p.getNumJoints(door)) @@ -31,8 +74,13 @@ prevTime = time.time() angle = math.pi*0.5 +count=0 while (1): - + count+=1 + if (count==12): + p.stopStateLogging(logId) + p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) + curTime = time.time() diff = curTime - prevTime @@ -45,5 +93,7 @@ while (1): p.setJointMotorControl2(door,1,p.POSITION_CONTROL, targetPosition = angle, positionGain=10.1, velocityGain=1, force=11.001) else: p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL, targetVelocity=1, force=1011) + #contacts = p.getContactPoints() + #print("contacts=",contacts) p.stepSimulation() - time.sleep(1./240.) + #time.sleep(1./240.) 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/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 9f226729e..7f800d40c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -456,7 +456,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P #ifdef BT_ENABLE_PHYSX case eCONNECT_PHYSX: { - sm = b3ConnectPhysX(); + sm = b3ConnectPhysX(argc, argv); break; } #endif