diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index cd821da1f..959e51a40 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -15,8 +15,8 @@ struct DrawGridData DrawGridData(int upAx = 1) : gridSize(10), - upOffset(0.001f), - upAxis(upAx) + upOffset(0.001f), + upAxis(upAx) { gridColor[0] = 0.6f; gridColor[1] = 0.6f; @@ -58,17 +58,17 @@ struct CommonGraphicsApp CommonGraphicsApp() : m_window(0), - m_renderer(0), - m_parameterInterface(0), - m_2dCanvasInterface(0), - m_leftMouseButton(false), - m_middleMouseButton(false), - m_rightMouseButton(false), - m_wheelMultiplier(0.01f), - m_mouseMoveMultiplier(0.4f), - m_mouseXpos(0.f), - m_mouseYpos(0.f), - m_mouseInitialized(false) + m_renderer(0), + m_parameterInterface(0), + m_2dCanvasInterface(0), + m_leftMouseButton(false), + m_middleMouseButton(false), + m_rightMouseButton(false), + m_wheelMultiplier(0.01f), + m_mouseMoveMultiplier(0.4f), + m_mouseXpos(0.f), + m_mouseYpos(0.f), + m_mouseInitialized(false) { m_backgroundColorRGB[0] = 0.7; m_backgroundColorRGB[1] = 0.7; @@ -126,13 +126,13 @@ struct CommonGraphicsApp virtual void drawText(const char* txt, int posX, int posY) { float size = 1; - float colorRGBA[4] = {0, 0, 0, 1}; + float colorRGBA[4] = { 0, 0, 0, 1 }; drawText(txt, posX, posY, size, colorRGBA); } virtual void drawText(const char* txt, int posX, int posY, float size) { - float colorRGBA[4] = {0, 0, 0, 1}; + float colorRGBA[4] = { 0, 0, 0, 1 }; drawText(txt, posX, posY, size, colorRGBA); } virtual void drawText(const char* txt, int posX, int posY, float size, float colorRGBA[4]) = 0; @@ -182,12 +182,12 @@ struct CommonGraphicsApp camera->getCameraPosition(camPos); b3Vector3 cameraPosition = b3MakeVector3(b3Scalar(camPos[0]), - b3Scalar(camPos[1]), - b3Scalar(camPos[2])); + b3Scalar(camPos[1]), + b3Scalar(camPos[2])); b3Vector3 cameraTargetPosition = b3MakeVector3(b3Scalar(targPos[0]), - b3Scalar(targPos[1]), - b3Scalar(targPos[2])); + b3Scalar(targPos[1]), + b3Scalar(targPos[2])); b3Vector3 cameraUp = b3MakeVector3(0, 0, 0); cameraUp[camera->getCameraUpAxis()] = 1.f; @@ -204,17 +204,17 @@ struct CommonGraphicsApp if (m_middleMouseButton) { - cameraTargetPosition += cameraUp * yDelta * 0.01; + cameraTargetPosition += cameraUp * yDelta *m_mouseMoveMultiplier* 0.01; b3Vector3 fwd = cameraTargetPosition - cameraPosition; b3Vector3 side = cameraUp.cross(fwd); side.normalize(); - cameraTargetPosition += side * xDelta * 0.01; + cameraTargetPosition += side * xDelta *m_mouseMoveMultiplier* 0.01; } if (m_rightMouseButton) { - cameraDistance -= xDelta * 0.01f; - cameraDistance -= yDelta * 0.01f; + cameraDistance -= xDelta * m_mouseMoveMultiplier*0.01f; + cameraDistance -= yDelta * m_mouseMoveMultiplier*0.01f; if (cameraDistance < 1) cameraDistance = 1; if (cameraDistance > 1000) @@ -251,7 +251,7 @@ struct CommonGraphicsApp float cameraDistance = camera->getCameraDistance(); if (deltay < 0 || cameraDistance > 1) { - cameraDistance -= deltay * 0.01f; + cameraDistance -= deltay*m_wheelMultiplier * 0.01f; if (cameraDistance < 1) cameraDistance = 1; camera->setCameraDistance(cameraDistance); diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index cfd4840e4..bb8671d48 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -1009,8 +1009,8 @@ bool OpenGLGuiHelper::getCameraInfo(int* width, int* height, float viewMatrix[16 { if (getRenderInterface() && getRenderInterface()->getActiveCamera()) { - *width = m_data->m_glApp->m_window->getWidth() * m_data->m_glApp->m_window->getRetinaScale(); - *height = m_data->m_glApp->m_window->getHeight() * m_data->m_glApp->m_window->getRetinaScale(); + *width = m_data->m_glApp->m_window->getWidth(); + *height = m_data->m_glApp->m_window->getHeight(); getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); getRenderInterface()->getActiveCamera()->getCameraUpVector(camUp); @@ -1043,12 +1043,12 @@ bool OpenGLGuiHelper::getCameraInfo(int* width, int* height, float viewMatrix[16 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_glApp->m_window->getRetinaScale(); - hor[1] = hori[1]* m_data->m_glApp->m_window->getRetinaScale(); - hor[2] = hori[2]* m_data->m_glApp->m_window->getRetinaScale(); - vert[0] = vertical[0]* m_data->m_glApp->m_window->getRetinaScale(); - vert[1] = vertical[1]* m_data->m_glApp->m_window->getRetinaScale(); - vert[2] = vertical[2]* m_data->m_glApp->m_window->getRetinaScale(); + hor[0] = hori[0]; + hor[1] = hori[1]; + hor[2] = hori[2]; + vert[0] = vertical[0]; + vert[1] = vertical[1]; + vert[2] = vertical[2]; *yaw = getRenderInterface()->getActiveCamera()->getCameraYaw(); *pitch = getRenderInterface()->getActiveCamera()->getCameraPitch(); diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index eac4a2dae..e41901674 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -2251,7 +2251,8 @@ void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, if (m_data->m_customVisualShapesConverter) { const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex); - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, inertialFrame, link, 0, colObj->getBroadphaseHandle()->getUid(), objectIndex, m_data->m_fileIO); + int uid3 = m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, inertialFrame, link, 0, colObj->getBroadphaseHandle()->getUid(), objectIndex, m_data->m_fileIO); + colObj->setUserIndex3(uid3); } } 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/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index bc9936f14..d6facb8f2 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -1589,7 +1589,15 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger) if (model.m_rootLinks.size() > 1) { - logger->reportWarning("URDF file with multiple root links found"); + std::string multipleRootMessage = + "URDF file with multiple root links found:"; + + for (int i = 0; i < model.m_rootLinks.size(); i++) + { + multipleRootMessage += " "; + multipleRootMessage += model.m_rootLinks[i]->m_name.c_str(); + } + logger->reportWarning(multipleRootMessage.c_str()); } if (model.m_rootLinks.size() == 0) 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/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e60828649..50bb4adef 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5403,7 +5403,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co int linkIndex = -1; if (bodyHandle->m_multiBody) { - int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex; + int linkIndex = clientCmd.m_requestRaycastIntersections.m_parentLinkIndex; if (linkIndex == -1) { tr = bodyHandle->m_multiBody->getBaseWorldTransform(); 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/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index 3d3183ef3..43d20387a 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -639,7 +639,7 @@ B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* cont case eZipFileIO: { #ifdef B3_USE_ZIPFILE_FILEIO - if (arguments->m_text) + if (arguments->m_text[0]) { result = obj->m_fileIO.addFileIOInterface(new ZipFileIO(eZipFileIO, arguments->m_text, &obj->m_fileIO)); } diff --git a/examples/pybullet/examples/createTexturedMeshVisualShape.py b/examples/pybullet/examples/createTexturedMeshVisualShape.py index 3dbc10c2e..6bb7646d5 100644 --- a/examples/pybullet/examples/createTexturedMeshVisualShape.py +++ b/examples/pybullet/examples/createTexturedMeshVisualShape.py @@ -125,7 +125,7 @@ indices=[ 0, 1, 2, 0, 2, 3, #//ground face 20, 21, 22, 20, 22, 23] #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) -visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices, uvs=uvs, normals=normals) +visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals) #visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices) #visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj") diff --git a/examples/pybullet/examples/createVisualShape.py b/examples/pybullet/examples/createVisualShape.py index 2d08ccdef..c833ef378 100644 --- a/examples/pybullet/examples/createVisualShape.py +++ b/examples/pybullet/examples/createVisualShape.py @@ -3,6 +3,7 @@ import time import math def getRayFromTo(mouseX,mouseY): + width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]] farPlane = 10000 @@ -15,9 +16,9 @@ def getRayFromTo(mouseX,mouseY): dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth] dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight] rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] - rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], - rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], - rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] + rayTo = [rayToCenter[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], + rayToCenter[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], + rayToCenter[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] return rayFrom,rayTo cid = p.connect(p.SHARED_MEMORY) @@ -66,7 +67,6 @@ while (1): hit = rayInfo[l] objectUid = hit[0] if (objectUid>=1): - #p.removeBody(objectUid) p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor]) currentColor+=1 if (currentColor>=len(colors)): 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 diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index e97bd71cc..23e163f0e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -70,13 +70,13 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl //solve featherstone frictional contact if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) { - for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++) + for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++) { if (iteration < infoGlobal.m_numIterations) { - int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; + int index = j1; - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; + btMultiBodySolverConstraint& frictionConstraint = m_multiBodySpinningFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; //adjust friction limits here if (totalImpulse > btScalar(0)) @@ -94,6 +94,41 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl } } + for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++) + { + if (iteration < infoGlobal.m_numIterations) + { + int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + j1++; + int index2 = j1; + btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints[index2]; + //adjust friction limits here + if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; + frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse); + frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse; + + btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB); + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + + if (frictionConstraintB.m_multiBodyA) + frictionConstraintB.m_multiBodyA->setPosUpdated(false); + if (frictionConstraintB.m_multiBodyB) + frictionConstraintB.m_multiBodyB->setPosUpdated(false); + } + } + } + for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++) { if (iteration < infoGlobal.m_numIterations) @@ -164,6 +199,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb m_multiBodyNormalContactConstraints.resize(0); m_multiBodyFrictionContactConstraints.resize(0); m_multiBodyTorsionalFrictionContactConstraints.resize(0); + m_multiBodySpinningFrictionContactConstraints.resize(0); m_data.m_jacobians.resize(0); m_data.m_deltaVelocitiesUnitImpulse.resize(0); @@ -1169,6 +1205,43 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalF return solverConstraint; } +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + BT_PROFILE("addMultiBodyRollingFrictionConstraint"); + + btMultiBodySolverConstraint& solverConstraint = m_multiBodySpinningFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); + return solverConstraint; +} void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); @@ -1258,7 +1331,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* { if (cp.m_combinedSpinningFriction > 0) { - addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal); + addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal); } if (cp.m_combinedRollingFriction > 0) { @@ -1267,11 +1340,8 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (cp.m_lateralFrictionDir1.length() > 0.001) - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); - - if (cp.m_lateralFrictionDir2.length() > 0.001) - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); } rollingFriction--; } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index f39f2879f..abf571883 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -34,6 +34,7 @@ protected: btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints; btMultiBodyJacobianData m_data; @@ -54,6 +55,10 @@ protected: btScalar combinedTorsionalFriction, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + btMultiBodySolverConstraint& addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow, btScalar * jacA, btScalar * jacB, btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,