Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #911 from erwincoumans/master

Browse Source 
Add pybullet setVRCameraState, allow 2 simultaneous shared memory connections (with a few restrictions)
This commit is contained in:
erwincoumans
2017-01-06 12:46:39 -08:00
committed by GitHub
parent 5b2ad7285c 4fc697f646
commit 1bd88cac69
15 changed files with 539 additions and 244 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
docs/pybullet_quickstartguide.pdf
View File

Binary file not shown.

16
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -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 <inttypes.h>
#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
}

15
examples/OpenGLWindow/SimpleCamera.cpp
View File

@@ -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;

2
examples/OpenGLWindow/SimpleCamera.h
View File

@@ -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();

51
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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;
}

6
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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

41
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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));

76
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -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()<endTime)
{
m_physicsServer.processClientCommands();
}
} else
{
//for (int i=0;i<10;i++)
m_physicsServer.processClientCommands();
}
#endif
{
if (m_multiThreadedHelper->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");

264
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@@ -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;i<MAX_SHARED_MEMORY_BLOCKS;i++)
{
m_testBlocks[i]=0;
m_areConnected[i]=false;
}
}
SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp)
SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp, int blockIndex)
{
SharedMemoryStatus& serverCmd =m_testBlock1->m_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;block<MAX_SHARED_MEMORY_BLOCKS;block++)
{
if (m_data->m_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;block<MAX_SHARED_MEMORY_BLOCKS;block++)
{
if (m_data->m_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;block<MAX_SHARED_MEMORY_BLOCKS;block++)
{
m_data->m_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);
}
}
}
}
}

95
examples/SharedMemory/PosixSharedMemory.cpp
View File

@@ -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<btSharedMemorySegment> 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;i<m_internalData->m_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;i<m_internalData->m_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
}

28
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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];
};

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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,

100
examples/SharedMemory/Win32SharedMemory.cpp
View File

@@ -2,22 +2,36 @@
#include "Win32SharedMemory.h"
#include "Bullet3Common/b3Logging.h"
#include "Bullet3Common/b3Scalar.h"
#include "LinearMath/btAlignedObjectArray.h"
#include <windows.h>
#include <stdio.h>
//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<Win32SharedMemorySegment> 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;i<m_internalData->m_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;i<m_internalData->m_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()

72
examples/pybullet/pybullet.c
View File

@@ -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. "

15
src/LinearMath/btAlignedObjectArray.h
View File

@@ -476,15 +476,18 @@ protected:
return index;
}
void removeAtIndex(int index)
{
if (index<size())
{
swap( index,size()-1);
pop_back();
}
}
void remove(const T& key)
{
int findIndex = findLinearSearch(key);
if (findIndex<size())
{
swap( findIndex,size()-1);
pop_back();
}
removeAtIndex(findIndex);
}
//PCK: whole function