mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 05:40:05 +00:00
Merge pull request #838 from erwincoumans/master
first version of 'getVisualShapeData' for pybullet / shmem API
This commit is contained in:
erwincoumans
GitHub
commit
ad07b3cf13
@ -1,8 +1,21 @@
|
||||
|
||||
IF NOT EXIST bin mkdir bin
|
||||
IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin
|
||||
|
||||
#optionally, hardcode the python path or
|
||||
#SET myvar=c:\python-3.5.3
|
||||
|
||||
#find a python version (hopefully just 1) and use this
|
||||
dir c:\python* /b /ad > tmp1234.txt
|
||||
|
||||
set /p myvar1= < tmp1234.txt
|
||||
set myvar=c:/%myvar1%
|
||||
del tmp1234.txt
|
||||
|
||||
#you can also override and hardcode the Python path like this (just remove the # hashmark in next line)
|
||||
#SET myvar=c:\python-3.5.2
|
||||
|
||||
cd build3
|
||||
|
||||
premake4 --double --enable_openvr --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --targetdir="../bin" vs2010
|
||||
premake4 --double --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010
|
||||
start vs2010
|
||||
|
||||
|
||||
Binary file not shown.
@ -1211,11 +1211,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
|
||||
{
|
||||
m_internalData->m_gui->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
|
||||
}
|
||||
if (gui2)
|
||||
{
|
||||
gui2->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
|
||||
}
|
||||
|
||||
|
||||
if (sUseOpenGL2)
|
||||
{
|
||||
restoreOpenGLState();
|
||||
|
||||
@ -228,7 +228,7 @@ void ImportSDFSetup::initPhysics()
|
||||
|
||||
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
|
||||
int rootLinkIndex = u2b.getRootLinkIndex();
|
||||
b3Printf("urdf root link index = %d\n",rootLinkIndex);
|
||||
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
|
||||
MyMultiBodyCreator creation(m_guiHelper);
|
||||
|
||||
u2b.getRootTransformInWorld(rootTrans);
|
||||
|
||||
@ -1144,13 +1144,13 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
|
||||
return false;
|
||||
}
|
||||
|
||||
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int objectIndex) const
|
||||
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
|
||||
{
|
||||
|
||||
if (m_data->m_customVisualShapesConverter)
|
||||
{
|
||||
const UrdfModel& model = m_data->m_urdfParser.getModel();
|
||||
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, objectIndex);
|
||||
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, bodyUniqueId);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -51,7 +51,7 @@ public:
|
||||
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
||||
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
|
||||
|
||||
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
|
||||
|
||||
|
||||
@ -238,7 +238,7 @@ void ImportUrdfSetup::initPhysics()
|
||||
|
||||
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
|
||||
int rootLinkIndex = u2b.getRootLinkIndex();
|
||||
b3Printf("urdf root link index = %d\n",rootLinkIndex);
|
||||
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
|
||||
MyMultiBodyCreator creation(m_guiHelper);
|
||||
|
||||
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
|
||||
|
||||
@ -136,8 +136,8 @@ void MultiDofDemo::initPhysics()
|
||||
bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
|
||||
bool multibodyOnly = false;
|
||||
bool canSleep = true;
|
||||
bool selfCollide = false;
|
||||
bool multibodyConstraint = true;
|
||||
bool selfCollide = true;
|
||||
bool multibodyConstraint = false;
|
||||
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
|
||||
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
|
||||
|
||||
@ -159,6 +159,7 @@ void MultiDofDemo::initPhysics()
|
||||
}
|
||||
//
|
||||
m_dynamicsWorld->setGravity(btVector3(0, -9.81 ,0));
|
||||
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
|
||||
//////////////////////////////////////////////
|
||||
if(numLinks > 0)
|
||||
{
|
||||
@ -247,8 +248,8 @@ void MultiDofDemo::initPhysics()
|
||||
frameInA.setIdentity();
|
||||
frameInB.setIdentity();
|
||||
btVector3 jointAxis(1.0,0.0,0.0);
|
||||
btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
|
||||
//btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB);
|
||||
//btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
|
||||
btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB);
|
||||
p2p->setMaxAppliedImpulse(2.0);
|
||||
m_dynamicsWorld->addMultiBodyConstraint(p2p);
|
||||
}
|
||||
@ -306,10 +307,10 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD
|
||||
for(int i = 0; i < numLinks; ++i)
|
||||
{
|
||||
if(!spherical)
|
||||
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
|
||||
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
|
||||
else
|
||||
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
|
||||
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
|
||||
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
|
||||
}
|
||||
|
||||
pMultiBody->finalizeMultiDof();
|
||||
|
||||
@ -48,6 +48,8 @@ public:
|
||||
|
||||
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
|
||||
|
||||
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
|
||||
|
||||
};
|
||||
|
||||
#endif // BT_PHYSICS_CLIENT_API_H
|
||||
|
||||
@ -1227,6 +1227,33 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con
|
||||
}
|
||||
|
||||
|
||||
|
||||
//request visual shape information
|
||||
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_REQUEST_VISUAL_SHAPE_INFO;
|
||||
command->m_requestVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueIdA;
|
||||
command->m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = 0;
|
||||
command->m_updateFlags = 0;
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||
if (cl)
|
||||
{
|
||||
cl->getCachedVisualShapeInformation(visualShapeInfo);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
|
||||
@ -93,7 +93,12 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
|
||||
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
|
||||
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
|
||||
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
|
||||
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData);
|
||||
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
|
||||
|
||||
//request visual shape information
|
||||
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
|
||||
void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
|
||||
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
|
||||
|
||||
@ -25,6 +25,11 @@ struct MyMotorInfo2
|
||||
int camVisualizerWidth = 320;//1024/3;
|
||||
int camVisualizerHeight = 240;//768/3;
|
||||
|
||||
enum CustomCommands
|
||||
{
|
||||
CMD_CUSTOM_SET_REALTIME_SIMULATION = CMD_MAX_CLIENT_COMMANDS+1,
|
||||
CMD_CUSTOM_SET_GRAVITY
|
||||
};
|
||||
|
||||
#define MAX_NUM_MOTORS 128
|
||||
|
||||
@ -306,7 +311,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
||||
for (int i = 0; i < numJoints; ++i) {
|
||||
struct b3JointSensorState sensorState;
|
||||
b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
|
||||
b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
|
||||
//b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
|
||||
}
|
||||
}
|
||||
break;
|
||||
@ -410,12 +415,20 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
||||
|
||||
break;
|
||||
}
|
||||
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: {
|
||||
case CMD_CUSTOM_SET_GRAVITY: {
|
||||
b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
|
||||
b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
|
||||
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_CUSTOM_SET_REALTIME_SIMULATION:
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
|
||||
b3PhysicsParamSetRealTimeSimulation(commandHandle,1);
|
||||
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_CALCULATE_INVERSE_DYNAMICS:
|
||||
{
|
||||
@ -458,6 +471,16 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
||||
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||
break;
|
||||
}
|
||||
case CMD_REQUEST_VISUAL_SHAPE_INFO:
|
||||
{
|
||||
if (m_selectedBody >= 0)
|
||||
{
|
||||
//request visual shape information
|
||||
b3SharedMemoryCommandHandle commandHandle = b3InitRequestVisualShapeInformation(m_physicsClientHandle, m_selectedBody);
|
||||
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Error("Unknown buttonId");
|
||||
@ -535,6 +558,8 @@ void PhysicsClientExample::createButtons()
|
||||
createButton("Save World",CMD_SAVE_WORLD, isTrigger);
|
||||
createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
|
||||
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
|
||||
createButton("Realtime Sim",CMD_CUSTOM_SET_REALTIME_SIMULATION, isTrigger);
|
||||
createButton("Get Visual Shape Info",CMD_REQUEST_VISUAL_SHAPE_INFO, isTrigger);
|
||||
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
|
||||
if (m_options!=eCLIENTEXAMPLE_SERVER)
|
||||
{
|
||||
@ -545,7 +570,7 @@ void PhysicsClientExample::createButtons()
|
||||
createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
|
||||
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
|
||||
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
|
||||
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
|
||||
createButton("Set gravity", CMD_CUSTOM_SET_GRAVITY, isTrigger);
|
||||
createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger);
|
||||
createButton("Get Contact Point Info", CMD_REQUEST_CONTACT_POINT_INFORMATION, isTrigger);
|
||||
|
||||
@ -584,7 +609,7 @@ void PhysicsClientExample::createButtons()
|
||||
{
|
||||
b3JointInfo info;
|
||||
b3GetJointInfo(m_physicsClientHandle,m_selectedBody,i,&info);
|
||||
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
|
||||
//b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
|
||||
|
||||
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
|
||||
{
|
||||
@ -959,7 +984,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
|
||||
{
|
||||
b3JointInfo info;
|
||||
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
|
||||
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
|
||||
//b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
|
||||
}
|
||||
|
||||
}
|
||||
@ -968,6 +993,18 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
|
||||
{
|
||||
b3Warning("Cannot get contact information");
|
||||
}
|
||||
if (statusType == CMD_VISUAL_SHAPE_INFO_FAILED)
|
||||
{
|
||||
b3Warning("Cannot get visual shape information");
|
||||
}
|
||||
|
||||
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
|
||||
{
|
||||
b3VisualShapeInformation shapeInfo;
|
||||
b3GetVisualShapeInformation(m_physicsClientHandle, &shapeInfo);
|
||||
b3Printf("Num visual shapes: %d", shapeInfo.m_numVisualShapes);
|
||||
}
|
||||
|
||||
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
|
||||
{
|
||||
b3ContactInformation contactPointData;
|
||||
|
||||
@ -40,6 +40,8 @@ struct PhysicsClientSharedMemoryInternalData {
|
||||
btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
|
||||
|
||||
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
|
||||
|
||||
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
|
||||
|
||||
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
|
||||
SharedMemoryStatus m_tempBackupServerStatus;
|
||||
@ -633,7 +635,28 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
b3Warning("Calculate Inverse Kinematics Request failed");
|
||||
break;
|
||||
}
|
||||
case CMD_VISUAL_SHAPE_INFO_COMPLETED:
|
||||
{
|
||||
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_testBlock1->m_bulletStreamDataServerToClientRefactor;
|
||||
for (int i = 0; i < numVisualShapesCopied; i++)
|
||||
{
|
||||
m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i];
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case CMD_VISUAL_SHAPE_INFO_FAILED:
|
||||
{
|
||||
b3Warning("Visual Shape Info Request failed");
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
b3Error("Unknown server status %d\n", serverCmd.m_type);
|
||||
btAssert(0);
|
||||
@ -710,6 +733,21 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
}
|
||||
}
|
||||
|
||||
if (serverCmd.m_type == CMD_VISUAL_SHAPE_INFO_COMPLETED)
|
||||
{
|
||||
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
||||
if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied)
|
||||
{
|
||||
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;
|
||||
submitClientCommand(command);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
|
||||
{
|
||||
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
||||
@ -813,6 +851,13 @@ void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3Contac
|
||||
|
||||
}
|
||||
|
||||
void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
|
||||
{
|
||||
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
|
||||
visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
|
||||
}
|
||||
|
||||
|
||||
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
|
||||
if (m_data->m_debugLinesFrom.size()) {
|
||||
return &m_data->m_debugLinesFrom[0].m_x;
|
||||
|
||||
@ -56,6 +56,8 @@ public:
|
||||
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
|
||||
|
||||
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
|
||||
|
||||
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
|
||||
};
|
||||
|
||||
#endif // BT_PHYSICS_CLIENT_API_H
|
||||
|
||||
@ -41,9 +41,9 @@ struct PhysicsDirectInternalData
|
||||
btAlignedObjectArray<int> m_cachedSegmentationMask;
|
||||
|
||||
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
|
||||
|
||||
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
|
||||
|
||||
|
||||
|
||||
PhysicsServerCommandProcessor* m_commandProcessor;
|
||||
|
||||
PhysicsDirectInternalData()
|
||||
@ -196,6 +196,44 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
|
||||
return m_data->m_hasStatus;
|
||||
}
|
||||
|
||||
bool PhysicsDirect::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);
|
||||
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)
|
||||
{
|
||||
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 PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
|
||||
{
|
||||
SharedMemoryCommand command = orgCommand;
|
||||
@ -380,6 +418,11 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
|
||||
return processContactPointData(command);
|
||||
}
|
||||
|
||||
if (command.m_type == CMD_REQUEST_VISUAL_SHAPE_INFO)
|
||||
{
|
||||
return processVisualShapeData(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)
|
||||
@ -576,3 +619,8 @@ void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation
|
||||
|
||||
}
|
||||
|
||||
void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
|
||||
{
|
||||
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
|
||||
visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
|
||||
}
|
||||
|
||||
@ -23,6 +23,8 @@ protected:
|
||||
|
||||
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
|
||||
|
||||
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
|
||||
|
||||
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
|
||||
|
||||
public:
|
||||
@ -74,6 +76,8 @@ public:
|
||||
|
||||
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
|
||||
|
||||
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
|
||||
|
||||
//those 2 APIs are for internal use for visualization
|
||||
virtual bool connect(struct GUIHelperInterface* guiHelper);
|
||||
virtual void renderScene();
|
||||
|
||||
@ -140,3 +140,8 @@ void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformati
|
||||
{
|
||||
return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);
|
||||
}
|
||||
|
||||
void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
|
||||
{
|
||||
return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo);
|
||||
}
|
||||
@ -62,6 +62,7 @@ public:
|
||||
|
||||
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
|
||||
|
||||
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
|
||||
};
|
||||
|
||||
#endif //PHYSICS_LOOP_BACK_H
|
||||
|
||||
@ -2971,7 +2971,36 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_REQUEST_VISUAL_SHAPE_INFO:
|
||||
{
|
||||
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_FAILED;
|
||||
//retrieve the visual shape information for a specific body
|
||||
|
||||
int totalNumVisualShapes = m_data->m_visualConverter.getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId);
|
||||
int totalBytesPerVisualShape = sizeof (b3VisualShapeData);
|
||||
int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
|
||||
b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient;
|
||||
|
||||
int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
|
||||
m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
|
||||
shapeIndex,
|
||||
visualShapeStoragePtr);
|
||||
|
||||
|
||||
//m_visualConverter
|
||||
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
|
||||
|
||||
serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED;
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Error("Unknown command encountered");
|
||||
|
||||
@ -151,6 +151,21 @@ struct RequestContactDataArgs
|
||||
int m_objectBIndexFilter;
|
||||
};
|
||||
|
||||
struct RequestVisualShapeDataArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
int m_startingVisualShapeIndex;
|
||||
};
|
||||
|
||||
struct SendVisualShapeDataArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
int m_startingVisualShapeIndex;
|
||||
int m_numVisualShapesCopied;
|
||||
int m_numRemainingVisualShapes;
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct SendDebugLinesArgs
|
||||
{
|
||||
@ -461,6 +476,7 @@ struct SharedMemoryCommand
|
||||
struct CalculateJacobianArgs m_calculateJacobianArguments;
|
||||
struct CreateJointArgs m_createJointArguments;
|
||||
struct RequestContactDataArgs m_requestContactPointArguments;
|
||||
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
|
||||
struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
|
||||
};
|
||||
};
|
||||
@ -496,6 +512,7 @@ struct SharedMemoryStatus
|
||||
struct CalculateJacobianResultArgs m_jacobianResultArgs;
|
||||
struct SendContactDataArgs m_sendContactPointArgs;
|
||||
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
|
||||
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@ -34,6 +34,7 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_CREATE_JOINT,
|
||||
CMD_REQUEST_CONTACT_POINT_INFORMATION,
|
||||
CMD_SAVE_WORLD,
|
||||
CMD_REQUEST_VISUAL_SHAPE_INFO,
|
||||
//don't go beyond this command!
|
||||
CMD_MAX_CLIENT_COMMANDS,
|
||||
|
||||
@ -78,8 +79,9 @@ enum EnumSharedMemoryServerStatus
|
||||
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
|
||||
CMD_SAVE_WORLD_COMPLETED,
|
||||
CMD_SAVE_WORLD_FAILED,
|
||||
|
||||
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
|
||||
CMD_VISUAL_SHAPE_INFO_COMPLETED,
|
||||
CMD_VISUAL_SHAPE_INFO_FAILED,
|
||||
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
|
||||
CMD_MAX_SERVER_COMMANDS
|
||||
};
|
||||
|
||||
@ -188,6 +190,25 @@ struct b3ContactInformation
|
||||
struct b3ContactPointData* m_contactPointData;
|
||||
};
|
||||
|
||||
#define VISUAL_SHAPE_MAX_PATH_LEN 128
|
||||
|
||||
struct b3VisualShapeData
|
||||
{
|
||||
int m_objectUniqueId;
|
||||
int m_linkIndex;
|
||||
int m_visualGeometryType;//box primitive, sphere primitive, triangle mesh
|
||||
double m_dimensions[3];//meaning depends on m_visualGeometryType
|
||||
char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN];
|
||||
double m_localInertiaFrame[7];//pos[3], orn[4]
|
||||
//todo: add more data if necessary (material color etc, although material can be in asset file .obj file)
|
||||
};
|
||||
|
||||
struct b3VisualShapeInformation
|
||||
{
|
||||
int m_numVisualShapes;
|
||||
struct b3VisualShapeData* m_visualShapeData;
|
||||
};
|
||||
|
||||
///b3LinkState provides extra information such as the Cartesian world coordinates
|
||||
///center of mass (COM) of the link, relative to the world reference frame.
|
||||
///Orientation is a quaternion x,y,z,w
|
||||
|
||||
@ -32,6 +32,7 @@ subject to the following restrictions:
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include "../Importers/ImportURDFDemo/UrdfParser.h"
|
||||
#include "../SharedMemory/SharedMemoryPublic.h"//for b3VisualShapeData
|
||||
|
||||
|
||||
enum MyFileType
|
||||
@ -60,6 +61,8 @@ struct TinyRendererVisualShapeConverterInternalData
|
||||
{
|
||||
|
||||
btHashMap<btHashPtr,TinyRendererObjectArray*> m_swRenderInstances;
|
||||
|
||||
btAlignedObjectArray<b3VisualShapeData> m_visualShapes;
|
||||
|
||||
int m_upAxis;
|
||||
int m_swWidth;
|
||||
@ -105,9 +108,14 @@ TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter()
|
||||
|
||||
|
||||
|
||||
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
|
||||
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
|
||||
{
|
||||
|
||||
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
|
||||
visualShapeOut.m_dimensions[0] = 0;
|
||||
visualShapeOut.m_dimensions[1] = 0;
|
||||
visualShapeOut.m_dimensions[2] = 0;
|
||||
visualShapeOut.m_meshAssetFileName[0] = 0;
|
||||
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
|
||||
@ -119,6 +127,9 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
||||
{
|
||||
btAlignedObjectArray<btVector3> vertices;
|
||||
|
||||
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_cylinderLength;
|
||||
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_cylinderRadius;
|
||||
|
||||
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
||||
int numSteps = 32;
|
||||
for (int i = 0; i<numSteps; i++)
|
||||
@ -140,7 +151,10 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
||||
}
|
||||
case URDF_GEOM_BOX:
|
||||
{
|
||||
|
||||
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_boxSize[0];
|
||||
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_boxSize[1];
|
||||
visualShapeOut.m_dimensions[2] = visual->m_geometry.m_boxSize[2];
|
||||
|
||||
btVector3 extents = visual->m_geometry.m_boxSize;
|
||||
|
||||
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
|
||||
@ -151,6 +165,8 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
||||
}
|
||||
case URDF_GEOM_SPHERE:
|
||||
{
|
||||
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius;
|
||||
|
||||
btScalar radius = visual->m_geometry.m_sphereRadius;
|
||||
btSphereShape* sphereShape = new btSphereShape(radius);
|
||||
convexColShape = sphereShape;
|
||||
@ -200,6 +216,18 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
||||
|
||||
|
||||
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
|
||||
|
||||
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0];
|
||||
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1];
|
||||
visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2];
|
||||
|
||||
int sl = strlen(fullPath);
|
||||
if (sl < (VISUAL_SHAPE_MAX_PATH_LEN-1))
|
||||
{
|
||||
memcpy(visualShapeOut.m_meshAssetFileName, fullPath, sl);
|
||||
visualShapeOut.m_meshAssetFileName[sl] = 0;
|
||||
}
|
||||
|
||||
FILE* f = fopen(fullPath, "rb");
|
||||
if (f)
|
||||
{
|
||||
@ -443,7 +471,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
||||
|
||||
|
||||
|
||||
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int objectIndex)
|
||||
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int bodyUniqueId)
|
||||
{
|
||||
|
||||
|
||||
@ -486,11 +514,23 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
|
||||
btAssert(visualsPtr);
|
||||
TinyRendererObjectArray* visuals = *visualsPtr;
|
||||
|
||||
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
|
||||
b3VisualShapeData visualShape;
|
||||
visualShape.m_objectUniqueId = bodyUniqueId;
|
||||
visualShape.m_linkIndex = linkIndex;
|
||||
visualShape.m_localInertiaFrame[0] = localInertiaFrame.getOrigin()[0];
|
||||
visualShape.m_localInertiaFrame[1] = localInertiaFrame.getOrigin()[1];
|
||||
visualShape.m_localInertiaFrame[2] = localInertiaFrame.getOrigin()[2];
|
||||
visualShape.m_localInertiaFrame[3] = localInertiaFrame.getRotation()[0];
|
||||
visualShape.m_localInertiaFrame[4] = localInertiaFrame.getRotation()[1];
|
||||
visualShape.m_localInertiaFrame[5] = localInertiaFrame.getRotation()[2];
|
||||
visualShape.m_localInertiaFrame[6] = localInertiaFrame.getRotation()[3];
|
||||
|
||||
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
|
||||
m_data->m_visualShapes.push_back(visualShape);
|
||||
|
||||
if (vertices.size() && indices.size())
|
||||
{
|
||||
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, objectIndex);
|
||||
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId);
|
||||
unsigned char* textureImage=0;
|
||||
int textureWidth=0;
|
||||
int textureHeight=0;
|
||||
@ -513,6 +553,64 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
|
||||
}
|
||||
}
|
||||
|
||||
int TinyRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId)
|
||||
{
|
||||
int start = -1;
|
||||
//find first one, then count how many
|
||||
for (int i = 0; i < m_data->m_visualShapes.size(); i++)
|
||||
{
|
||||
if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId)
|
||||
{
|
||||
start = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
int count = 0;
|
||||
|
||||
if (start >= 0)
|
||||
{
|
||||
for (int i = start; i < m_data->m_visualShapes.size(); i++)
|
||||
{
|
||||
if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId)
|
||||
{
|
||||
count++;
|
||||
}
|
||||
else
|
||||
{
|
||||
//storage of each visual shape for a given body unique id assumed to be contiguous
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
int TinyRendererVisualShapeConverter::getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData)
|
||||
{
|
||||
int start = -1;
|
||||
//find first one, then count how many
|
||||
for (int i = 0; i < m_data->m_visualShapes.size(); i++)
|
||||
{
|
||||
if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId)
|
||||
{
|
||||
start = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
int count = 0;
|
||||
|
||||
if (start >= 0)
|
||||
{
|
||||
if (start + shapeIndex < m_data->m_visualShapes.size())
|
||||
{
|
||||
*shapeData = m_data->m_visualShapes[start + shapeIndex];
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void TinyRendererVisualShapeConverter::setUpAxis(int axis)
|
||||
{
|
||||
m_data->m_upAxis = axis;
|
||||
|
||||
@ -4,6 +4,9 @@
|
||||
|
||||
#include "../Importers/ImportURDFDemo/LinkVisualShapesConverter.h"
|
||||
|
||||
|
||||
|
||||
|
||||
struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
|
||||
{
|
||||
|
||||
@ -15,6 +18,10 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
|
||||
|
||||
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex);
|
||||
|
||||
virtual int getNumVisualShapes(int bodyUniqueId);
|
||||
|
||||
virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData);
|
||||
|
||||
void setUpAxis(int axis);
|
||||
|
||||
void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ);
|
||||
|
||||
@ -1260,6 +1260,107 @@ static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
|
||||
{
|
||||
int size = PySequence_Size(args);
|
||||
int objectUniqueId = -1;
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
struct b3VisualShapeInformation visualShapeInfo;
|
||||
int statusType;
|
||||
int i;
|
||||
PyObject* pyResultList = 0;
|
||||
|
||||
if (size == 1)
|
||||
{
|
||||
if (!PyArg_ParseTuple(args, "i", &objectUniqueId)) {
|
||||
PyErr_SetString(SpamError, "Error parsing object unique id");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
commandHandle = b3InitRequestVisualShapeInformation(sm, objectUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
|
||||
{
|
||||
b3GetVisualShapeInformation(sm, &visualShapeInfo);
|
||||
pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes);
|
||||
for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++)
|
||||
{
|
||||
PyObject* visualShapeObList = PyTuple_New(7);
|
||||
PyObject* item;
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId);
|
||||
PyTuple_SetItem(visualShapeObList, 0, item);
|
||||
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_linkIndex);
|
||||
PyTuple_SetItem(visualShapeObList, 1, item);
|
||||
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_visualGeometryType);
|
||||
PyTuple_SetItem(visualShapeObList, 2, item);
|
||||
|
||||
{
|
||||
PyObject* vec = PyTuple_New(3);
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[0]);
|
||||
PyTuple_SetItem(vec, 0, item);
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[1]);
|
||||
PyTuple_SetItem(vec, 1, item);
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[2]);
|
||||
PyTuple_SetItem(vec, 2, item);
|
||||
PyTuple_SetItem(visualShapeObList, 3, vec);
|
||||
}
|
||||
|
||||
item = PyString_FromString(visualShapeInfo.m_visualShapeData[i].m_meshAssetFileName);
|
||||
PyTuple_SetItem(visualShapeObList, 4, item);
|
||||
|
||||
{
|
||||
PyObject* vec = PyTuple_New(3);
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[0]);
|
||||
PyTuple_SetItem(vec, 0, item);
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[1]);
|
||||
PyTuple_SetItem(vec, 1, item);
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[2]);
|
||||
PyTuple_SetItem(vec, 2, item);
|
||||
PyTuple_SetItem(visualShapeObList, 5, vec);
|
||||
}
|
||||
|
||||
{
|
||||
PyObject* vec = PyTuple_New(4);
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[3]);
|
||||
PyTuple_SetItem(vec, 0, item);
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[4]);
|
||||
PyTuple_SetItem(vec, 1, item);
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[5]);
|
||||
PyTuple_SetItem(vec, 2, item);
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[6]);
|
||||
PyTuple_SetItem(vec, 3, item);
|
||||
PyTuple_SetItem(visualShapeObList, 6, vec);
|
||||
}
|
||||
|
||||
|
||||
|
||||
PyTuple_SetItem(pyResultList, i, visualShapeObList);
|
||||
}
|
||||
return pyResultList;
|
||||
}
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError, "Error receiving visual shape info");
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError, "getVisualShapeData requires 1 argument (object unique id)");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
|
||||
int size = PySequence_Size(args);
|
||||
int objectUniqueIdA = -1;
|
||||
@ -1287,7 +1388,6 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
|
||||
commandHandle = b3InitRequestContactPointInformation(sm);
|
||||
b3SetContactFilterBodyA(commandHandle, objectUniqueIdA);
|
||||
b3SetContactFilterBodyB(commandHandle, objectUniqueIdB);
|
||||
b3SubmitClientCommand(sm, commandHandle);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
@ -2194,6 +2294,9 @@ static PyMethodDef SpamMethods[] = {
|
||||
"object-object collisions. Optional arguments one or two object unique "
|
||||
"ids, that need to be involved in the contact."},
|
||||
|
||||
{ "getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,
|
||||
"Return the visual shape information for one object." },
|
||||
|
||||
{"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
|
||||
"Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to "
|
||||
"quaternion [x,y,z,w]"},
|
||||
|
||||
@ -67,6 +67,20 @@ void testSharedMemory(b3PhysicsClientHandle sm)
|
||||
numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
|
||||
ASSERT_EQ(numBodies,1);
|
||||
bodyUniqueId = bodyIndicesOut[0];
|
||||
{
|
||||
{
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle command = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
|
||||
{
|
||||
struct b3VisualShapeInformation vi;
|
||||
b3GetVisualShapeInformation(sm, &vi);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
numJoints = b3GetNumJoints(sm,bodyUniqueId);
|
||||
ASSERT_EQ(numJoints,7);
|
||||
@ -119,6 +133,8 @@ void testSharedMemory(b3PhysicsClientHandle sm)
|
||||
|
||||
if (bodyIndex>=0)
|
||||
{
|
||||
|
||||
|
||||
numJoints = b3GetNumJoints(sm,bodyIndex);
|
||||
for (i=0;i<numJoints;i++)
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user