mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 21:30:09 +00:00
db008ab3c2
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
317 lines
9.4 KiB
C++
317 lines
9.4 KiB
C++
#include "PhysicsServerSharedMemory.h"
|
|
#include "PosixSharedMemory.h"
|
|
#include "Win32SharedMemory.h"
|
|
|
|
#include "../CommonInterfaces/CommonRenderInterface.h"
|
|
|
|
#include "btBulletDynamicsCommon.h"
|
|
|
|
#include "LinearMath/btTransform.h"
|
|
|
|
#include "Bullet3Common/b3Logging.h"
|
|
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
|
|
#include "SharedMemoryBlock.h"
|
|
|
|
#include "PhysicsServerCommandProcessor.h"
|
|
|
|
//number of shared memory blocks == number of simultaneous connections
|
|
#define MAX_SHARED_MEMORY_BLOCKS 2
|
|
|
|
struct PhysicsServerSharedMemoryInternalData
|
|
{
|
|
|
|
///end handle management
|
|
|
|
|
|
SharedMemoryInterface* m_sharedMemory;
|
|
bool m_ownsSharedMemory;
|
|
|
|
SharedMemoryBlock* m_testBlocks[MAX_SHARED_MEMORY_BLOCKS];
|
|
int m_sharedMemoryKey;
|
|
bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS];
|
|
bool m_verboseOutput;
|
|
PhysicsServerCommandProcessor* m_commandProcessor;
|
|
|
|
PhysicsServerSharedMemoryInternalData()
|
|
:m_sharedMemory(0),
|
|
m_ownsSharedMemory(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, int blockIndex)
|
|
{
|
|
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,int blockIndex)
|
|
{
|
|
m_testBlocks[blockIndex]->m_numServerCommands++;
|
|
}
|
|
|
|
};
|
|
|
|
|
|
PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* sharedMem)
|
|
{
|
|
m_data = new PhysicsServerSharedMemoryInternalData();
|
|
if (sharedMem)
|
|
{
|
|
m_data->m_sharedMemory = sharedMem;
|
|
m_data->m_ownsSharedMemory = false;
|
|
} else
|
|
{
|
|
#ifdef _WIN32
|
|
m_data->m_sharedMemory = new Win32SharedMemoryServer();
|
|
#else
|
|
m_data->m_sharedMemory = new PosixSharedMemory();
|
|
#endif
|
|
m_data->m_ownsSharedMemory = true;
|
|
}
|
|
|
|
m_data->m_commandProcessor = new PhysicsServerCommandProcessor;
|
|
|
|
|
|
}
|
|
|
|
PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
|
|
{
|
|
|
|
m_data->m_commandProcessor->deleteDynamicsWorld();
|
|
delete m_data->m_commandProcessor;
|
|
|
|
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()
|
|
{
|
|
m_data->m_commandProcessor->deleteDynamicsWorld();
|
|
m_data->m_commandProcessor ->createEmptyDynamicsWorld();
|
|
}
|
|
void PhysicsServerSharedMemory::setSharedMemoryKey(int key)
|
|
{
|
|
m_data->m_sharedMemoryKey = key;
|
|
}
|
|
|
|
|
|
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper)
|
|
{
|
|
|
|
m_data->m_commandProcessor->setGuiHelper(guiHelper);
|
|
|
|
|
|
bool allowCreation = true;
|
|
bool allConnected = false;
|
|
int numConnected = 0;
|
|
|
|
|
|
int counter = 0;
|
|
for (int block=0;block<MAX_SHARED_MEMORY_BLOCKS;block++)
|
|
{
|
|
if (m_data->m_areConnected[block])
|
|
{
|
|
allConnected = true;
|
|
numConnected++;
|
|
b3Warning("connectSharedMemory, while already connected");
|
|
continue;
|
|
}
|
|
do
|
|
{
|
|
|
|
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;
|
|
numConnected++;
|
|
} 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");
|
|
}
|
|
}
|
|
|
|
allConnected = (numConnected==MAX_SHARED_MEMORY_BLOCKS);
|
|
|
|
return allConnected;
|
|
}
|
|
|
|
|
|
void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
|
|
{
|
|
m_data->m_commandProcessor->deleteDynamicsWorld();
|
|
|
|
m_data->m_commandProcessor->setGuiHelper(0);
|
|
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("releaseSharedMemory1\n");
|
|
}
|
|
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()
|
|
{
|
|
disconnectSharedMemory(true);
|
|
}
|
|
|
|
|
|
void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents)
|
|
{
|
|
m_data->m_commandProcessor->stepSimulationRealTime(dtInSec,vrEvents, numVREvents, keyEvents,numKeyEvents);
|
|
}
|
|
|
|
void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)
|
|
{
|
|
m_data->m_commandProcessor->enableRealTimeSimulation(enableRealTimeSim);
|
|
}
|
|
|
|
|
|
|
|
void PhysicsServerSharedMemory::processClientCommands()
|
|
{
|
|
for (int block = 0;block<MAX_SHARED_MEMORY_BLOCKS;block++)
|
|
{
|
|
|
|
if (m_data->m_areConnected[block] && m_data->m_testBlocks[block])
|
|
{
|
|
m_data->m_commandProcessor->replayLogCommand(&m_data->m_testBlocks[block]->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
|
|
|
|
///we ignore overflow of integer for now
|
|
if (m_data->m_testBlocks[block]->m_numClientCommands> m_data->m_testBlocks[block]->m_numProcessedClientCommands)
|
|
{
|
|
|
|
//BT_PROFILE("processClientCommand");
|
|
|
|
//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);
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void PhysicsServerSharedMemory::renderScene(int renderFlags)
|
|
{
|
|
m_data->m_commandProcessor->renderScene(renderFlags);
|
|
|
|
}
|
|
|
|
void PhysicsServerSharedMemory::syncPhysicsToGraphics()
|
|
{
|
|
m_data->m_commandProcessor->syncPhysicsToGraphics();
|
|
}
|
|
|
|
void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags)
|
|
{
|
|
m_data->m_commandProcessor->physicsDebugDraw(debugDrawFlags);
|
|
}
|
|
|
|
|
|
bool PhysicsServerSharedMemory::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
|
|
{
|
|
return m_data->m_commandProcessor->pickBody(rayFromWorld,rayToWorld);
|
|
}
|
|
|
|
bool PhysicsServerSharedMemory::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
|
|
{
|
|
return m_data->m_commandProcessor->movePickedBody(rayFromWorld,rayToWorld);
|
|
}
|
|
void PhysicsServerSharedMemory::removePickingConstraint()
|
|
{
|
|
m_data->m_commandProcessor->removePickingConstraint();
|
|
}
|
|
|
|
void PhysicsServerSharedMemory::enableCommandLogging(bool enable, const char* fileName)
|
|
{
|
|
m_data->m_commandProcessor->enableCommandLogging(enable,fileName);
|
|
}
|
|
|
|
void PhysicsServerSharedMemory::replayFromLogFile(const char* fileName)
|
|
{
|
|
m_data->m_commandProcessor->replayFromLogFile(fileName);
|
|
}
|