diff --git a/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf index a46bbf128..41d4a60b2 100644 Binary files a/docs/pybullet_quickstartguide.pdf and b/docs/pybullet_quickstartguide.pdf differ diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index b5af22122..3fe496020 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -162,7 +162,12 @@ FILE* gTimingFile = 0; #ifndef __STDC_FORMAT_MACROS #define __STDC_FORMAT_MACROS #endif //__STDC_FORMAT_MACROS + +//see http://stackoverflow.com/questions/18107426/printf-format-for-unsigned-int64-on-windows +#ifndef _WIN32 #include +#endif + #define BT_TIMING_CAPACITY 16*65536 static bool m_firstTiming = true; @@ -246,12 +251,21 @@ struct btTimings char newname[1024]; static int counter2=0; - sprintf(newname,"%s%d",name,counter2++); + +#ifdef _WIN32 + + fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%I64d.%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n", + threadId, startTimeDiv1000,startTimeRem1000Str, newname); + fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%I64d.%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}", + threadId, endTimeDiv1000,endTimeRem1000Str,newname); + +#else fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n", threadId, startTimeDiv1000,startTimeRem1000Str, newname); fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}", threadId, endTimeDiv1000,endTimeRem1000Str,newname); +#endif #endif } diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp index f0ba375d2..4b0b27164 100644 --- a/examples/OpenGLWindow/SimpleCamera.cpp +++ b/examples/OpenGLWindow/SimpleCamera.cpp @@ -71,6 +71,21 @@ void SimpleCamera::setVRCamera(const float viewMat[16], const float projectionMa } } +bool SimpleCamera::getVRCamera(float viewMat[16], float projectionMatrix[16]) +{ + if (m_data->m_enableVR) + { + for (int i=0;i<16;i++) + { + viewMat[i] = m_data->m_viewMatrixVR[i]; + projectionMatrix[i] = m_data->m_projectionMatrixVR[i]; + } + } + return false; +} + + + void SimpleCamera::disableVRCamera() { m_data->m_enableVR = false; diff --git a/examples/OpenGLWindow/SimpleCamera.h b/examples/OpenGLWindow/SimpleCamera.h index 7221f01a6..5a61a729f 100644 --- a/examples/OpenGLWindow/SimpleCamera.h +++ b/examples/OpenGLWindow/SimpleCamera.h @@ -15,6 +15,8 @@ struct SimpleCamera : public CommonCameraInterface virtual void getCameraViewMatrix(float m[16]) const; virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16]); + virtual bool getVRCamera(float viewMat[16], float projectionMatrix[16]); + virtual void setVRCameraOffsetTransform(const float offset[16]); virtual void disableVRCamera(); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e30c1519c..56fe09ee6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2191,3 +2191,54 @@ void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* cl->getCachedVREvents(vrEventsData); } } + +b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_SET_VR_CAMERA_STATE; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle)command; + +} + +int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE); + command->m_updateFlags |= VR_CAMERA_ROOT_POSITION; + command->m_vrCameraStateArguments.m_rootPosition[0] = rootPos[0]; + command->m_vrCameraStateArguments.m_rootPosition[1] = rootPos[1]; + command->m_vrCameraStateArguments.m_rootPosition[2] = rootPos[2]; + return 0; + +} + +int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE); + command->m_updateFlags |= VR_CAMERA_ROOT_ORIENTATION; + command->m_vrCameraStateArguments.m_rootOrientation[0] = rootOrn[0]; + command->m_vrCameraStateArguments.m_rootOrientation[1] = rootOrn[1]; + command->m_vrCameraStateArguments.m_rootOrientation[2] = rootOrn[2]; + return 0; +} + +int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE); + command->m_updateFlags |= VR_CAMERA_ROOT_TRACKING_OBJECT; + command->m_vrCameraStateArguments.m_trackingObjectUniqueId = objectUniqueId; + return 0; +} + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b8b27dd53..2f553ea73 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -314,6 +314,12 @@ int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, dou b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient); void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData); +b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient); +int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]); +int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]); +int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); + + #ifdef __cplusplus diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 911a501f3..7f98b8dfb 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -44,11 +44,14 @@ bool gEnableRealTimeSimVR=false; bool gCreateDefaultRobotAssets = false; int gInternalSimFlags = 0; bool gResetSimulation = 0; -int gHuskyId = -1; -btTransform huskyTr = btTransform::getIdentity(); +int gVRTrackingObjectUniqueId = -1; +btTransform gVRTrackingObjectTr = btTransform::getIdentity(); int gCreateObjectSimVR = -1; int gEnableKukaControl = 0; +btVector3 gVRTeleportPos1(0,0,0); +btQuaternion gVRTeleportOrn(0, 0, 0,1); + btScalar simTimeScalingFactor = 1; btScalar gRhsClamp = 1.f; @@ -1369,6 +1372,32 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } #endif + case CMD_SET_VR_CAMERA_STATE: + { + + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_POSITION) + { + gVRTeleportPos1[0] = clientCmd.m_vrCameraStateArguments.m_rootPosition[0]; + gVRTeleportPos1[1] = clientCmd.m_vrCameraStateArguments.m_rootPosition[1]; + gVRTeleportPos1[2] = clientCmd.m_vrCameraStateArguments.m_rootPosition[2]; + } + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_ORIENTATION) + { + gVRTeleportOrn[0] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[0]; + gVRTeleportOrn[1] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[1]; + gVRTeleportOrn[2] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[2]; + gVRTeleportOrn[3] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[3]; + } + + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_TRACKING_OBJECT) + { + gVRTrackingObjectUniqueId = clientCmd.m_vrCameraStateArguments.m_trackingObjectUniqueId; + } + + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + break; + } case CMD_REQUEST_VR_EVENTS_DATA: { serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0; @@ -4287,12 +4316,12 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const gSubStep = m_data->m_physicsDeltaTime; } - if (gHuskyId >= 0) + if (gVRTrackingObjectUniqueId >= 0) { - InternalBodyHandle* bodyHandle = m_data->getHandle(gHuskyId); + InternalBodyHandle* bodyHandle = m_data->getHandle(gVRTrackingObjectUniqueId); if (bodyHandle && bodyHandle->m_multiBody) { - huskyTr = bodyHandle->m_multiBody->getBaseWorldTransform(); + gVRTrackingObjectTr = bodyHandle->m_multiBody->getBaseWorldTransform(); } } @@ -4595,8 +4624,6 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() //loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - gHuskyId = bodyId; - b3Printf("huskyId = %d", gHuskyId); m_data->m_huskyId = bodyId; m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index ca4d9e204..994312986 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -26,9 +26,14 @@ //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet! extern btVector3 gLastPickPos; -btVector3 gVRTeleportPos1(0,0,0); + +btVector3 gVRTeleportPosLocal(0,0,0); +btQuaternion gVRTeleportOrnLocal(0,0,0,1); + +extern btVector3 gVRTeleportPos1; +extern btQuaternion gVRTeleportOrn; btScalar gVRTeleportRotZ = 0; -btQuaternion gVRTeleportOrn(0, 0, 0,1); + extern btVector3 gVRGripperPos; extern btQuaternion gVRGripperOrn; extern btVector3 gVRController2Pos; @@ -57,9 +62,9 @@ static void saveCurrentSettingsVR() FILE* f = fopen(startFileNameVR, "w"); if (f) { - fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]); - fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]); - fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]); + fprintf(f, "--camPosX= %f\n", gVRTeleportPosLocal[0]); + fprintf(f, "--camPosY= %f\n", gVRTeleportPosLocal[1]); + fprintf(f, "--camPosZ= %f\n", gVRTeleportPosLocal[2]); fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ); fclose(f); } @@ -109,9 +114,9 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void if (message->at(1) == 16) { gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2)); - gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); + gVRTeleportOrnLocal = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); saveCurrentSettingsVR(); -// b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ); +// b3Printf("gVRTeleportOrnLocal rotZ = %f\n", gVRTeleportRotZ); } if (message->at(1) == 32) @@ -123,9 +128,9 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void { if (message->at(1) == i) { - gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2)); + gVRTeleportPosLocal[i] = getParamf(-2, 2, message->at(2)); saveCurrentSettingsVR(); -// b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]); +// b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPosLocal[i]); } } @@ -1040,19 +1045,19 @@ public: setSharedMemoryKey(shmemKey); } - if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0])) + if (args.GetCmdLineArgument("camPosX", gVRTeleportPosLocal[0])) { - printf("camPosX=%f\n", gVRTeleportPos1[0]); + printf("camPosX=%f\n", gVRTeleportPosLocal[0]); } - if (args.GetCmdLineArgument("camPosY", gVRTeleportPos1[1])) + if (args.GetCmdLineArgument("camPosY", gVRTeleportPosLocal[1])) { - printf("camPosY=%f\n", gVRTeleportPos1[1]); + printf("camPosY=%f\n", gVRTeleportPosLocal[1]); } - if (args.GetCmdLineArgument("camPosZ", gVRTeleportPos1[2])) + if (args.GetCmdLineArgument("camPosZ", gVRTeleportPosLocal[2])) { - printf("camPosZ=%f\n", gVRTeleportPos1[2]); + printf("camPosZ=%f\n", gVRTeleportPosLocal[2]); } float camRotZ = 0.f; @@ -1060,7 +1065,7 @@ public: { printf("camRotZ = %f\n", camRotZ); btQuaternion ornZ(btVector3(0, 0, 1), camRotZ); - gVRTeleportOrn = ornZ; + gVRTeleportOrnLocal = ornZ; } if (args.CheckCmdLineFlag("robotassets")) @@ -1439,23 +1444,7 @@ void PhysicsServerExample::stepSimulation(float deltaTime) - #if 0 - if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK) - { - btClock rtc; - btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800); - - while (rtc.getTimeMilliseconds()m_childGuiHelper->getRenderInterface()) { @@ -1474,8 +1463,8 @@ extern int gDroppedSimulationSteps; extern int gNumSteps; extern double gDtInSec; extern double gSubStep; -extern int gHuskyId; -extern btTransform huskyTr; +extern int gVRTrackingObjectUniqueId; +extern btTransform gVRTrackingObjectTr; struct LineSegment { @@ -1607,15 +1596,22 @@ void PhysicsServerExample::drawUserDebugLines() void PhysicsServerExample::renderScene() { + btTransform vrTrans; + gVRTeleportPos1 = gVRTeleportPosLocal; + gVRTeleportOrn = gVRTeleportOrnLocal; -#if 0 ///little VR test to follow/drive Husky vehicle - if (gHuskyId >= 0) + if (gVRTrackingObjectUniqueId >= 0) { - gVRTeleportPos1 = huskyTr.getOrigin(); - gVRTeleportOrn = huskyTr.getRotation(); + btTransform vrTrans; + vrTrans.setOrigin(gVRTeleportPosLocal); + vrTrans.setRotation(gVRTeleportOrnLocal); + + vrTrans = vrTrans * gVRTrackingObjectTr; + + gVRTeleportPos1 = vrTrans.getOrigin(); + gVRTeleportOrn = vrTrans.getRotation(); } -#endif B3_PROFILE("PhysicsServerExample::RenderScene"); diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index 5c967c9c4..f19620cbd 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -14,7 +14,8 @@ #include "PhysicsServerCommandProcessor.h" - +//number of shared memory blocks == number of simultaneous connections +#define MAX_SHARED_MEMORY_BLOCKS 2 struct PhysicsServerSharedMemoryInternalData { @@ -25,36 +26,39 @@ struct PhysicsServerSharedMemoryInternalData SharedMemoryInterface* m_sharedMemory; bool m_ownsSharedMemory; - SharedMemoryBlock* m_testBlock1; + SharedMemoryBlock* m_testBlocks[MAX_SHARED_MEMORY_BLOCKS]; int m_sharedMemoryKey; - bool m_isConnected; + bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS]; bool m_verboseOutput; PhysicsServerCommandProcessor* m_commandProcessor; PhysicsServerSharedMemoryInternalData() :m_sharedMemory(0), m_ownsSharedMemory(false), - m_testBlock1(0), - m_sharedMemoryKey(SHARED_MEMORY_KEY), - m_isConnected(false), - m_verboseOutput(false), + m_sharedMemoryKey(SHARED_MEMORY_KEY), + m_verboseOutput(false), m_commandProcessor(0) { - + + for (int i=0;im_serverCommands[0]; + SharedMemoryStatus& serverCmd =m_testBlocks[blockIndex]->m_serverCommands[0]; serverCmd .m_type = statusType; serverCmd.m_sequenceNumber = sequenceNumber; serverCmd.m_timeStamp = timeStamp; return serverCmd; } - void submitServerStatus(SharedMemoryStatus& status) + void submitServerStatus(SharedMemoryStatus& status,int blockIndex) { - m_testBlock1->m_numServerCommands++; + m_testBlocks[blockIndex]->m_numServerCommands++; } }; @@ -84,9 +88,25 @@ PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* shar PhysicsServerSharedMemory::~PhysicsServerSharedMemory() { + m_data->m_commandProcessor->deleteDynamicsWorld(); delete m_data->m_commandProcessor; - delete m_data; + + if (m_data->m_sharedMemory) + { + if (m_data->m_verboseOutput) + { + b3Printf("m_sharedMemory\n"); + } + if (m_data->m_ownsSharedMemory) + { + delete m_data->m_sharedMemory; + } + m_data->m_sharedMemory = 0; + } + + + delete m_data; } void PhysicsServerSharedMemory::resetDynamicsWorld() @@ -107,55 +127,61 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* bool allowCreation = true; + bool allConnected = false; + - if (m_data->m_isConnected) - { - b3Warning("connectSharedMemory, while already connected"); - return m_data->m_isConnected; - } + int counter = 0; - do - { + for (int block=0;blockm_areConnected[block]) + { + allConnected = true; + b3Warning("connectSharedMemory, while already connected"); + continue; + } + do + { - m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation); - if (m_data->m_testBlock1) - { - int magicId =m_data->m_testBlock1->m_magicId; - if (m_data->m_verboseOutput) - { - b3Printf("magicId = %d\n", magicId); - } - - if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) - { - InitSharedMemoryBlock(m_data->m_testBlock1); - if (m_data->m_verboseOutput) - { - b3Printf("Created and initialized shared memory block\n"); - } - m_data->m_isConnected = true; - } else - { - m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); - m_data->m_testBlock1 = 0; - m_data->m_isConnected = false; - } - } else - { - b3Error("Cannot connect to shared memory"); - m_data->m_isConnected = false; - } - } while (counter++ < 10 && !m_data->m_isConnected); - - if (!m_data->m_isConnected) - { - b3Error("Server cannot connect to shared memory.\n"); - } + m_data->m_testBlocks[block] = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey+block, SHARED_MEMORY_SIZE,allowCreation); + if (m_data->m_testBlocks[block]) + { + int magicId =m_data->m_testBlocks[block]->m_magicId; + if (m_data->m_verboseOutput) + { + b3Printf("magicId = %d\n", magicId); + } + + if (m_data->m_testBlocks[block]->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) + { + InitSharedMemoryBlock(m_data->m_testBlocks[block]); + if (m_data->m_verboseOutput) + { + b3Printf("Created and initialized shared memory block\n"); + } + m_data->m_areConnected[block] = true; + } else + { + m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey+block, SHARED_MEMORY_SIZE); + m_data->m_testBlocks[block] = 0; + m_data->m_areConnected[block] = false; + } + } else + { + b3Error("Cannot connect to shared memory"); + m_data->m_areConnected[block] = false; + } + } while (counter++ < 10 && !m_data->m_areConnected[block]); + if (!m_data->m_areConnected[block]) + { + b3Error("Server cannot connect to shared memory.\n"); + } + } - return m_data->m_isConnected; + return allConnected; } @@ -167,72 +193,34 @@ void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMe { b3Printf("releaseSharedMemory1\n"); } - if (m_data->m_testBlock1) - { - if (m_data->m_verboseOutput) - { - b3Printf("m_testBlock1\n"); - } - if (deInitializeSharedMemory) - { - m_data->m_testBlock1->m_magicId = 0; - if (m_data->m_verboseOutput) - { - b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlock1->m_magicId); - } - } - btAssert(m_data->m_sharedMemory); - m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); - } - if (m_data->m_sharedMemory) - { - if (m_data->m_verboseOutput) - { - b3Printf("m_sharedMemory\n"); - } - if (m_data->m_ownsSharedMemory) - { - delete m_data->m_sharedMemory; - } - m_data->m_sharedMemory = 0; - m_data->m_testBlock1 = 0; - } + for (int block = 0;blockm_testBlocks[block]) + { + if (m_data->m_verboseOutput) + { + b3Printf("m_testBlock1\n"); + } + if (deInitializeSharedMemory) + { + m_data->m_testBlocks[block]->m_magicId = 0; + if (m_data->m_verboseOutput) + { + b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlocks[block]->m_magicId); + } + } + btAssert(m_data->m_sharedMemory); + m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey+block, SHARED_MEMORY_SIZE); + } + m_data->m_testBlocks[block] = 0; + m_data->m_areConnected[block] = false; + + } } void PhysicsServerSharedMemory::releaseSharedMemory() { - if (m_data->m_verboseOutput) - { - b3Printf("releaseSharedMemory1\n"); - } - if (m_data->m_testBlock1) - { - if (m_data->m_verboseOutput) - { - b3Printf("m_testBlock1\n"); - } - m_data->m_testBlock1->m_magicId = 0; - if (m_data->m_verboseOutput) - { - b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId); - } - btAssert(m_data->m_sharedMemory); - m_data->m_sharedMemory->releaseSharedMemory( m_data->m_sharedMemoryKey -, SHARED_MEMORY_SIZE); - } - if (m_data->m_sharedMemory) - { - if (m_data->m_verboseOutput) - { - b3Printf("m_sharedMemory\n"); - } - if (m_data->m_ownsSharedMemory) - { - delete m_data->m_sharedMemory; - } - m_data->m_sharedMemory = 0; - m_data->m_testBlock1 = 0; - } + disconnectSharedMemory(true); } @@ -250,30 +238,34 @@ void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim) void PhysicsServerSharedMemory::processClientCommands() { - if (m_data->m_isConnected && m_data->m_testBlock1) + for (int block = 0;blockm_commandProcessor->replayLogCommand(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - ///we ignore overflow of integer for now - if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands) + if (m_data->m_areConnected[block] && m_data->m_testBlocks[block]) { - - - //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands - btAssert(m_data->m_testBlock1->m_numClientCommands==m_data->m_testBlock1->m_numProcessedClientCommands+1); + m_data->m_commandProcessor->replayLogCommand(&m_data->m_testBlocks[block]->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; + ///we ignore overflow of integer for now + if (m_data->m_testBlocks[block]->m_numClientCommands> m_data->m_testBlocks[block]->m_numProcessedClientCommands) + { + - m_data->m_testBlock1->m_numProcessedClientCommands++; - //todo, timeStamp - int timeStamp = 0; - SharedMemoryStatus& serverStatusOut = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - bool hasStatus = m_data->m_commandProcessor->processCommand(clientCmd, serverStatusOut,&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - if (hasStatus) - { - m_data->submitServerStatus(serverStatusOut); - } - + //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands + btAssert(m_data->m_testBlocks[block]->m_numClientCommands==m_data->m_testBlocks[block]->m_numProcessedClientCommands+1); + + const SharedMemoryCommand& clientCmd =m_data->m_testBlocks[block]->m_clientCommands[0]; + + m_data->m_testBlocks[block]->m_numProcessedClientCommands++; + //todo, timeStamp + int timeStamp = 0; + SharedMemoryStatus& serverStatusOut = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp,block); + bool hasStatus = m_data->m_commandProcessor->processCommand(clientCmd, serverStatusOut,&m_data->m_testBlocks[block]->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + m_data->submitServerStatus(serverStatusOut,block); + } + + } } } } diff --git a/examples/SharedMemory/PosixSharedMemory.cpp b/examples/SharedMemory/PosixSharedMemory.cpp index 0297d43e2..9c16b3fe7 100644 --- a/examples/SharedMemory/PosixSharedMemory.cpp +++ b/examples/SharedMemory/PosixSharedMemory.cpp @@ -1,6 +1,7 @@ #include "PosixSharedMemory.h" #include "Bullet3Common/b3Logging.h" #include "LinearMath/btScalar.h" //for btAssert +#include "LinearMath/btAlignedObjectArray.h" //Windows implementation is in Win32SharedMemory.cpp #ifndef _WIN32 @@ -16,16 +17,28 @@ #endif +struct btSharedMemorySegment +{ + int m_key; + int m_sharedMemoryId; + void* m_sharedMemoryPtr; + bool m_createdSharedMemory; + + btSharedMemorySegment() + : m_sharedMemoryId(-1), + m_sharedMemoryPtr(0), + m_createdSharedMemory(true) + { + } + +}; + struct PosixSharedMemoryInteralData { - bool m_createdSharedMemory; - int m_sharedMemoryId; - void* m_sharedMemoryPtr; - + btAlignedObjectArray m_segments; + + PosixSharedMemoryInteralData() - :m_createdSharedMemory(false), - m_sharedMemoryId(-1), - m_sharedMemoryPtr(0) { } }; @@ -53,6 +66,27 @@ struct btPointerCaster void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCreation) { #ifdef TEST_SHARED_MEMORY + + { + btSharedMemorySegment* seg = 0; + int i=0; + + for (i=0;im_segments.size();i++) + { + if (m_internalData->m_segments[i].m_key == key) + { + seg = &m_internalData->m_segments[i]; + break; + } + } + if (seg) + { + b3Error("already created shared memory segment using same key"); + return seg->m_sharedMemoryPtr; + } + + } + int flags = (allowCreation ? IPC_CREAT : 0) | 0666; int id = shmget((key_t) key, (size_t) size,flags); if (id < 0) @@ -67,9 +101,12 @@ void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCr b3Error("shmat returned -1"); } else { - m_internalData->m_createdSharedMemory = allowCreation; - m_internalData->m_sharedMemoryId = id; - m_internalData->m_sharedMemoryPtr = result.ptr; + btSharedMemorySegment seg; + seg.m_key = key; + seg.m_createdSharedMemory = allowCreation; + seg.m_sharedMemoryId = id; + seg.m_sharedMemoryPtr = result.ptr; + m_internalData->m_segments.push_back(seg); return result.ptr; } } @@ -82,14 +119,33 @@ void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCr void PosixSharedMemory::releaseSharedMemory(int key, int size) { #ifdef TEST_SHARED_MEMORY - if (m_internalData->m_sharedMemoryId < 0) + + btSharedMemorySegment* seg = 0; + int i=0; + + for (i=0;im_segments.size();i++) + { + if (m_internalData->m_segments[i].m_key == key) + { + seg = &m_internalData->m_segments[i]; + break; + } + } + + if (0==seg) + { + b3Error("PosixSharedMemory::releaseSharedMemory: shared memory key not found"); + return; + } + + if (seg->m_sharedMemoryId < 0) { b3Error("PosixSharedMemory::releaseSharedMemory: shared memory id is not set"); } else { - if (m_internalData->m_createdSharedMemory) + if (seg->m_createdSharedMemory) { - int result = shmctl(m_internalData->m_sharedMemoryId,IPC_RMID,0); + int result = shmctl(seg->m_sharedMemoryId,IPC_RMID,0); if (result == -1) { b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1"); @@ -97,15 +153,18 @@ void PosixSharedMemory::releaseSharedMemory(int key, int size) { b3Printf("PosixSharedMemory::releaseSharedMemory removed shared memory"); } - m_internalData->m_createdSharedMemory = false; - m_internalData->m_sharedMemoryId = -1; + seg->m_createdSharedMemory = false; + seg->m_sharedMemoryId = -1; } - if (m_internalData->m_sharedMemoryPtr) + if (seg->m_sharedMemoryPtr) { - shmdt(m_internalData->m_sharedMemoryPtr); - m_internalData->m_sharedMemoryPtr = 0; + shmdt(seg->m_sharedMemoryPtr); + seg->m_sharedMemoryPtr = 0; b3Printf("PosixSharedMemory::releaseSharedMemory detached shared memory\n"); } } + + m_internalData->m_segments.removeAtIndex(i); + #endif } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 4efee8023..cd22178f3 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -594,6 +594,26 @@ struct UserDebugDrawResultArgs int m_debugItemUniqueId; }; +struct SendVREvents +{ + int m_numVRControllerEvents; + b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS]; +}; + +enum eVRCameraEnums +{ + VR_CAMERA_ROOT_POSITION=1, + VR_CAMERA_ROOT_ORIENTATION=2, + VR_CAMERA_ROOT_TRACKING_OBJECT=4 +}; + +struct VRCameraState +{ + double m_rootPosition[3]; + double m_rootOrientation[4]; + int m_trackingObjectUniqueId; +}; + struct SharedMemoryCommand { @@ -635,6 +655,7 @@ struct SharedMemoryCommand struct UserDebugDrawArgs m_userDebugDrawArgs; struct RequestRaycastIntersections m_requestRaycastIntersections; struct LoadBunnyArgs m_loadBunnyArguments; + struct VRCameraState m_vrCameraStateArguments; }; }; @@ -657,11 +678,8 @@ struct SendOverlappingObjectsArgs int m_numRemainingOverlappingObjects; }; -struct SendVREvents -{ - int m_numVRControllerEvents; - b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS]; -}; + + diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7eb9d37ad..a517659bb 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -46,6 +46,8 @@ enum EnumSharedMemoryClientCommand CMD_SET_SHADOW, CMD_USER_DEBUG_DRAW, CMD_REQUEST_VR_EVENTS_DATA, + CMD_SET_VR_CAMERA_STATE, + //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, diff --git a/examples/SharedMemory/Win32SharedMemory.cpp b/examples/SharedMemory/Win32SharedMemory.cpp index 5e95f9be0..7a2c688ea 100644 --- a/examples/SharedMemory/Win32SharedMemory.cpp +++ b/examples/SharedMemory/Win32SharedMemory.cpp @@ -2,22 +2,36 @@ #include "Win32SharedMemory.h" #include "Bullet3Common/b3Logging.h" #include "Bullet3Common/b3Scalar.h" +#include "LinearMath/btAlignedObjectArray.h" #include #include //see also https://msdn.microsoft.com/en-us/library/windows/desktop/aa366551%28v=vs.85%29.aspx -struct Win32SharedMemoryInteralData +struct Win32SharedMemorySegment { + int m_key; HANDLE m_hMapFile; - void* m_buf; TCHAR m_szName[1024]; + + Win32SharedMemorySegment() + :m_hMapFile(0), + m_buf(0), + m_key(-1) + { + m_szName[0] = 0; + } + +}; + +struct Win32SharedMemoryInteralData +{ + btAlignedObjectArray m_segments; + Win32SharedMemoryInteralData() - :m_hMapFile(0), - m_buf(0) { } }; @@ -33,32 +47,53 @@ Win32SharedMemory::~Win32SharedMemory() void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreation) { - b3Assert(m_internalData->m_buf==0); + { + + Win32SharedMemorySegment* seg = 0; + int i=0; + + for (i=0;im_segments.size();i++) + { + if (m_internalData->m_segments[i].m_key == key) + { + seg = &m_internalData->m_segments[i]; + break; + } + } + if (seg) + { + b3Error("already created shared memory segment using same key"); + return seg->m_buf; + } + } + + Win32SharedMemorySegment seg; + seg.m_key = key; #ifdef UNICODE - swprintf_s (m_internalData->m_szName,TEXT("MyFileMappingObject%d"),key); + swprintf_s (seg.m_szName,TEXT("MyFileMappingObject%d"),key); #else - sprintf(m_internalData->m_szName,"MyFileMappingObject%d",key); + sprintf(seg.m_szName,"MyFileMappingObject%d",key); #endif - m_internalData->m_hMapFile = OpenFileMapping( + seg.m_hMapFile = OpenFileMapping( FILE_MAP_ALL_ACCESS, // read/write access FALSE, // do not inherit the name - m_internalData->m_szName); // name of mapping object + seg.m_szName); // name of mapping object - if (m_internalData->m_hMapFile==NULL) + if (seg.m_hMapFile==NULL) { if (allowCreation) { - m_internalData->m_hMapFile = CreateFileMapping( + seg.m_hMapFile = CreateFileMapping( INVALID_HANDLE_VALUE, // use paging file NULL, // default security PAGE_READWRITE, // read/write access 0, // maximum object size (high-order DWORD) size, // maximum object size (low-order DWORD) - m_internalData->m_szName); // name of mapping object + seg.m_szName); // name of mapping object } else { b3Warning("Could not create file mapping object (%d).\n",GetLastError()); @@ -67,37 +102,56 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre } - m_internalData->m_buf = MapViewOfFile(m_internalData->m_hMapFile, // handle to map object + seg.m_buf = MapViewOfFile(seg.m_hMapFile, // handle to map object FILE_MAP_ALL_ACCESS, // read/write permission 0, 0, size); - if (m_internalData->m_buf == NULL) + if (seg.m_buf == NULL) { b3Warning("Could not map view of file (%d).\n",GetLastError()); - CloseHandle(m_internalData->m_hMapFile); + CloseHandle(seg.m_hMapFile); return 0; } - return m_internalData->m_buf; + m_internalData->m_segments.push_back(seg); + return seg.m_buf; } void Win32SharedMemory::releaseSharedMemory(int key, int size) { - if (m_internalData->m_buf) - { + Win32SharedMemorySegment* seg = 0; + int i=0; + + for (i=0;im_segments.size();i++) + { + if (m_internalData->m_segments[i].m_key == key) + { + seg = &m_internalData->m_segments[i]; + break; + } + } - UnmapViewOfFile(m_internalData->m_buf); - m_internalData->m_buf=0; + if (seg==0) + { + b3Error("Couldn't find shared memory segment"); + return; } - if (m_internalData->m_hMapFile) + if (seg->m_buf) { - CloseHandle(m_internalData->m_hMapFile); - m_internalData->m_hMapFile = 0; + UnmapViewOfFile(seg->m_buf); + seg->m_buf=0; } + if (seg->m_hMapFile) + { + CloseHandle(seg->m_hMapFile); + seg->m_hMapFile = 0; + } + + m_internalData->m_segments.removeAtIndex(i); } Win32SharedMemoryServer::Win32SharedMemoryServer() diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index da4cf300b..4fed9e3a5 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1174,6 +1174,8 @@ static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* k double gravZ = -10.0; int ret; b3PhysicsClientHandle sm = 0; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; int physicsClientId = 0; static char *kwlist[] = { "gravX", "gravY", "gravZ", "physicsClientId", NULL }; @@ -1189,9 +1191,8 @@ static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* k } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; - + command = b3InitPhysicsParamCommand(sm); + ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ); // ret = b3PhysicsParamSetTimeStep(command, timeStep); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -2431,6 +2432,56 @@ static PyObject* pybullet_getMatrixFromQuaterion(PyObject* self, PyObject* args) }; +static PyObject* pybullet_setVRCameraState(PyObject* self, PyObject* args, PyObject *keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + PyObject* rootPosObj=0; + PyObject* rootOrnObj=0; + int trackObjectUid=-2; + double rootPos[3]; + double rootOrn[4]; + + static char *kwlist[] = {"rootPosition", "rootOrientation", "trackObject", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|OOii", kwlist,&rootPosObj, &rootOrnObj, &trackObjectUid,&physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3SetVRCameraStateCommandInit(sm); + + if (pybullet_internalSetVectord(rootPosObj,rootPos)) + { + b3SetVRCameraRootPosition(commandHandle,rootPos); + } + if (pybullet_internalSetVector4d(rootOrnObj,rootOrn)) + { + b3SetVRCameraRootOrientation(commandHandle,rootOrn); + } + + if (trackObjectUid>=-1) + { + b3SetVRCameraTrackingObject(commandHandle,trackObjectUid); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + Py_INCREF(Py_None); + return Py_None; + +} + + static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject *keywds) { b3SharedMemoryCommandHandle commandHandle; @@ -3732,10 +3783,10 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) { static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args,PyObject* keywds) { { - int objectUniqueId, linkIndex, flags; + int objectUniqueId=-1, linkIndex=-1, flags; double force[3]; double position[3] = {0.0, 0.0, 0.0}; - PyObject* forceObj, *posObj; + PyObject* forceObj=0, *posObj=0; b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; @@ -4304,15 +4355,15 @@ static PyMethodDef SpamMethods[] = { "Set a single joint motor control mode and desired target value. There is " "no immediate state change, stepSimulation will process the motors."}, - {"applyExternalForce",(PyCFunction) pybullet_applyExternalForce, METH_VARARGS, + {"applyExternalForce",(PyCFunction) pybullet_applyExternalForce, METH_VARARGS| METH_KEYWORDS, "for objectUniqueId, linkIndex (-1 for base/root link), apply a force " "[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or " - "FORCE_IN_WORLD_FRAME coordinates"}, + "WORLD_FRAME coordinates"}, {"applyExternalTorque", (PyCFunction)pybullet_applyExternalTorque, METH_VARARGS| METH_KEYWORDS, "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque " "[x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or " - "TORQUE_IN_WORLD_FRAME coordinates"}, + "WORLD_FRAME coordinates"}, {"renderImage", pybullet_renderImageObsolete, METH_VARARGS, "obsolete, please use getCameraImage and getViewProjectionMatrices instead" @@ -4420,6 +4471,11 @@ static PyMethodDef SpamMethods[] = { { "getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS, "Get Virtual Reality events, for example to track VR controllers position/buttons" }, + { "setVRCameraState", (PyCFunction)pybullet_setVRCameraState, METH_VARARGS | METH_KEYWORDS, + "Set properties of the VR Camera such as its root transform " + "for teleporting or to track objects (camera inside a vehicle for example)." + }, + { "rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS, "Cast a ray and return the first object hit, if any. " diff --git a/src/LinearMath/btAlignedObjectArray.h b/src/LinearMath/btAlignedObjectArray.h index 146ae72e8..56f8189c1 100644 --- a/src/LinearMath/btAlignedObjectArray.h +++ b/src/LinearMath/btAlignedObjectArray.h @@ -476,15 +476,18 @@ protected: return index; } + void removeAtIndex(int index) + { + if (index