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

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2016-11-29 14:17:38 -08:00
parent 0cb2b21b5f 5e52760fbd
commit e8ff969a6f
25 changed files with 931 additions and 264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Evolution/NN3DWalkers.cpp
View File

@@ -1035,6 +1035,7 @@ void NN3DWalkersExample::drawMarkings() {
}
void NN3DWalkersExample::printWalkerConfigs(){
#if 0
char configString[25 + NUM_WALKERS*BODYPART_COUNT*JOINT_COUNT*(3+15+1) + NUM_WALKERS*4 + 1]; // 15 precision + [],\n
char* runner = configString;
sprintf(runner,"Population configuration:");
@@ -1058,4 +1059,5 @@ void NN3DWalkersExample::printWalkerConfigs(){
}
runner[0] = '\0';
b3Printf(configString);
#endif
}

2
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -1167,7 +1167,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
}
BT_PROFILE("Render Scene");
sCurrentDemo->renderScene();
}
} else
{
B3_PROFILE("physicsDebugDraw");
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );

109
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -672,6 +672,40 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan
return 0;
}
int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_BASE_LINEAR_VELOCITY;
command->m_initPoseArgs.m_hasInitialStateQdot[0] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[1] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[2] = 1;
command->m_initPoseArgs.m_initialStateQdot[0] = linVel[0];
command->m_initPoseArgs.m_initialStateQdot[1] = linVel[1];
command->m_initPoseArgs.m_initialStateQdot[2] = linVel[2];
return 0;
}
int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_BASE_ANGULAR_VELOCITY;
command->m_initPoseArgs.m_hasInitialStateQdot[3] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[4] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[5] = 1;
command->m_initPoseArgs.m_initialStateQdot[3] = angVel[0];
command->m_initPoseArgs.m_initialStateQdot[4] = angVel[1];
command->m_initPoseArgs.m_initialStateQdot[5] = angVel[2];
return 0;
}
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -686,6 +720,8 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
return 0;
}
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -1195,6 +1231,46 @@ int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle)
return status->m_userDebugDrawArgs.m_debugItemUniqueId;
}
b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_DEBUG_DRAW;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
command->m_updateFlags |= USER_DEBUG_SET_CUSTOM_OBJECT_COLOR;
command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId;
command->m_userDebugDrawArgs.m_linkIndex = linkIndex;
command->m_userDebugDrawArgs.m_objectDebugColorRGB[0] = objectColorRGB[0];
command->m_userDebugDrawArgs.m_objectDebugColorRGB[1] = objectColorRGB[1];
command->m_userDebugDrawArgs.m_objectDebugColorRGB[2] = objectColorRGB[2];
}
void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
command->m_updateFlags |= USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR;
command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId;
command->m_userDebugDrawArgs.m_linkIndex = linkIndex;
}
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient)
@@ -1533,6 +1609,8 @@ b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClient
command->m_requestContactPointArguments.m_startingContactPointIndex = 0;
command->m_requestContactPointArguments.m_objectAIndexFilter = -1;
command->m_requestContactPointArguments.m_objectBIndexFilter = -1;
command->m_requestContactPointArguments.m_linkIndexAIndexFilter = -2;
command->m_requestContactPointArguments.m_linkIndexBIndexFilter = -2;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
@@ -1545,6 +1623,37 @@ void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int body
command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA;
}
void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER;
command->m_requestContactPointArguments.m_linkIndexAIndexFilter= linkIndexA;
}
void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER;
command->m_requestContactPointArguments.m_linkIndexBIndexFilter = linkIndexB;
}
void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
{
b3SetContactFilterLinkA(commandHandle, linkIndexA);
}
void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
{
b3SetContactFilterLinkB(commandHandle, linkIndexB);
}
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

13
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -89,9 +89,15 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle p
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime);
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId);
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient);
void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]);
void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
///All debug items unique Ids are positive: a negative unique Id means failure.
int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle);
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
@@ -126,12 +132,16 @@ void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle comm
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
///compute the closest points between two bodies
b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient);
void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
@@ -248,6 +258,9 @@ int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, do
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]);
int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]);
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);

112
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -542,7 +542,7 @@ struct PhysicsServerCommandProcessorInternalData
m_kukaGripperMultiBody(0),
m_kukaGripperRevolute1(0),
m_kukaGripperRevolute2(0),
m_allowRealTimeSimulation(true),
m_allowRealTimeSimulation(false),
m_huskyId(-1),
m_KukaId(-1),
m_sphereId(-1),
@@ -2352,6 +2352,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
btVector3 baseLinVel(0, 0, 0);
btVector3 baseAngVel(0, 0, 0);
if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
{
baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0],
clientCmd.m_initPoseArgs.m_initialStateQdot[1],
clientCmd.m_initPoseArgs.m_initialStateQdot[2]);
mb->setBaseVel(baseLinVel);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
{
baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3],
clientCmd.m_initPoseArgs.m_initialStateQdot[4],
clientCmd.m_initPoseArgs.m_initialStateQdot[5]);
mb->setBaseOmega(baseAngVel);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
{
btVector3 zero(0,0,0);
@@ -2359,7 +2381,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]);
mb->setBaseVel(zero);
mb->setBaseVel(baseLinVel);
mb->setBasePos(btVector3(
clientCmd.m_initPoseArgs.m_initialStateQ[0],
clientCmd.m_initPoseArgs.m_initialStateQ[1],
@@ -2372,7 +2394,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
mb->setBaseOmega(btVector3(0,0,0));
mb->setBaseOmega(baseAngVel);
btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3],
clientCmd.m_initPoseArgs.m_initialStateQ[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5],
@@ -2784,6 +2806,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter;
int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter;
bool hasLinkIndexAFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER));
bool hasLinkIndexBFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER));
int linkIndexA = clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter;
int linkIndexB = clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter;
btAlignedObjectArray<btCollisionObject*> setA;
btAlignedObjectArray<btCollisionObject*> setB;
btAlignedObjectArray<int> setALinkIndex;
@@ -2798,15 +2826,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
if (bodyA->m_multiBody->getBaseCollider())
{
setA.push_back(bodyA->m_multiBody->getBaseCollider());
setALinkIndex.push_back(-1);
if (!hasLinkIndexAFilter || (linkIndexA == -1))
{
setA.push_back(bodyA->m_multiBody->getBaseCollider());
setALinkIndex.push_back(-1);
}
}
for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++)
{
if (bodyA->m_multiBody->getLink(i).m_collider)
{
setA.push_back(bodyA->m_multiBody->getLink(i).m_collider);
setALinkIndex.push_back(i);
if (!hasLinkIndexAFilter || (linkIndexA == i))
{
setA.push_back(bodyA->m_multiBody->getLink(i).m_collider);
setALinkIndex.push_back(i);
}
}
}
}
@@ -2826,15 +2860,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
if (bodyB->m_multiBody->getBaseCollider())
{
setB.push_back(bodyB->m_multiBody->getBaseCollider());
setBLinkIndex.push_back(-1);
if (!hasLinkIndexBFilter || (linkIndexB == -1))
{
setB.push_back(bodyB->m_multiBody->getBaseCollider());
setBLinkIndex.push_back(-1);
}
}
for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++)
{
if (bodyB->m_multiBody->getLink(i).m_collider)
{
setB.push_back(bodyB->m_multiBody->getLink(i).m_collider);
setBLinkIndex.push_back(i);
if (!hasLinkIndexBFilter || (linkIndexB ==i))
{
setB.push_back(bodyB->m_multiBody->getLink(i).m_collider);
setBLinkIndex.push_back(i);
}
}
}
}
@@ -3528,7 +3568,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
for( int i=0;i<numRb;i++)
{
btCollisionObject* colObj = importer->getRigidBodyByIndex(i);
if (colObj)
{
btRigidBody* rb = btRigidBody::upcast(colObj);
@@ -3591,7 +3630,54 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
hasStatus = true;
if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR))
{
int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body)
{
btCollisionObject* destColObj = 0;
if (body->m_multiBody)
{
if (clientCmd.m_userDebugDrawArgs.m_linkIndex == -1)
{
destColObj = body->m_multiBody->getBaseCollider();
}
else
{
if (clientCmd.m_userDebugDrawArgs.m_linkIndex >= 0 && clientCmd.m_userDebugDrawArgs.m_linkIndex < body->m_multiBody->getNumLinks())
{
destColObj = body->m_multiBody->getLink(clientCmd.m_userDebugDrawArgs.m_linkIndex).m_collider;
}
}
}
if (body->m_rigidBody)
{
destColObj = body->m_rigidBody;
}
if (destColObj)
{
if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)
{
destColObj->removeCustomDebugColor();
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
}
if (clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR)
{
btVector3 objectColorRGB;
objectColorRGB.setValue(clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[0],
clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[1],
clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[2]);
destColObj->setCustomDebugColor(objectColorRGB);
serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
}
}
}
}
if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT)
{

95
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -825,6 +825,7 @@ public:
virtual bool wantsTermination();
virtual bool isConnected();
virtual void renderScene();
void drawUserDebugLines();
virtual void exitPhysics();
virtual void physicsDebugDraw(int debugFlags);
@@ -1353,7 +1354,51 @@ extern int gHuskyId;
extern btTransform huskyTr;
void PhysicsServerExample::drawUserDebugLines()
{
static char line0[1024];
static char line1[1024];
//draw all user-debug-lines
//add array of lines
//draw all user- 'text3d' messages
if (m_multiThreadedHelper)
{
for (int i = 0; i<m_multiThreadedHelper->m_userDebugLines.size(); i++)
{
btVector3 from;
from.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]);
btVector3 toX;
toX.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]);
btVector3 color;
color.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
}
for (int i = 0; i<m_multiThreadedHelper->m_userDebugText.size(); i++)
{
m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2],
m_multiThreadedHelper->m_userDebugText[i].textSize);
}
}
}
void PhysicsServerExample::renderScene()
{
@@ -1369,48 +1414,8 @@ void PhysicsServerExample::renderScene()
B3_PROFILE("PhysicsServerExample::RenderScene");
static char line0[1024];
static char line1[1024];
//draw all user-debug-lines
//add array of lines
//draw all user- 'text3d' messages
if (m_multiThreadedHelper)
{
for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
{
btVector3 from;
from.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]);
btVector3 toX;
toX.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]);
btVector3 color;
color.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
}
for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
{
m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2],
m_multiThreadedHelper->m_userDebugText[i].textSize);
}
}
drawUserDebugLines();
if (gEnableRealTimeSimVR)
{
@@ -1424,6 +1429,7 @@ void PhysicsServerExample::renderScene()
static int count = 0;
count++;
#if 0
if (0 == (count & 1))
{
btScalar curTime = m_clock.getTimeSeconds();
@@ -1444,6 +1450,7 @@ void PhysicsServerExample::renderScene()
worseFps = 1000000;
}
}
#endif
#ifdef BT_ENABLE_VR
if ((gInternalSimFlags&2 ) && m_tinyVrGui==0)
@@ -1468,7 +1475,9 @@ void PhysicsServerExample::renderScene()
tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
b3Scalar dt = 0.01;
m_tinyVrGui->clearTextArea();
static char line0[1024];
static char line1[1024];
m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);
@@ -1544,6 +1553,8 @@ void PhysicsServerExample::renderScene()
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
{
drawUserDebugLines();
///debug rendering
m_physicsServer.physicsDebugDraw(debugDrawFlags);

19
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -109,7 +109,9 @@ enum EnumInitPoseFlags
{
INIT_POSE_HAS_INITIAL_POSITION=1,
INIT_POSE_HAS_INITIAL_ORIENTATION=2,
INIT_POSE_HAS_JOINT_STATE=4
INIT_POSE_HAS_JOINT_STATE=4,
INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8,
INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16,
};
@@ -122,6 +124,8 @@ struct InitPoseArgs
int m_bodyUniqueId;
int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQ[MAX_DEGREE_OF_FREEDOM];
int m_hasInitialStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQdot[MAX_DEGREE_OF_FREEDOM];
};
@@ -160,6 +164,8 @@ enum EnumRequestContactDataUpdateFlags
{
CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE=1,
CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD=2,
CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER = 4,
CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER = 8,
};
struct RequestContactDataArgs
@@ -167,6 +173,8 @@ struct RequestContactDataArgs
int m_startingContactPointIndex;
int m_objectAIndexFilter;
int m_objectBIndexFilter;
int m_linkIndexAIndexFilter;
int m_linkIndexBIndexFilter;
double m_closestDistanceThreshold;
int m_mode;
};
@@ -526,7 +534,10 @@ enum EnumUserDebugDrawFlags
USER_DEBUG_HAS_LINE=1,
USER_DEBUG_HAS_TEXT=2,
USER_DEBUG_REMOVE_ONE_ITEM=4,
USER_DEBUG_REMOVE_ALL=8
USER_DEBUG_REMOVE_ALL=8,
USER_DEBUG_SET_CUSTOM_OBJECT_COLOR = 16,
USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR = 32,
};
struct UserDebugDrawArgs
@@ -543,6 +554,10 @@ struct UserDebugDrawArgs
double m_textPositionXYZ[3];
double m_textColorRGB[3];
double m_textSize;
double m_objectDebugColorRGB[3];
int m_objectUniqueId;
int m_linkIndex;
};

2
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -172,6 +172,7 @@ m_objectIndex(-1)
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
@@ -193,6 +194,7 @@ m_objectIndex(objectIndex)
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();

580
examples/pybullet/pybullet.c
View File

@@ -30,6 +30,123 @@ enum eCONNECT_METHOD {
static PyObject* SpamError;
static b3PhysicsClientHandle sm = 0;
static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
double v = 0.0;
PyObject* item;
if (PyList_Check(seq)) {
item = PyList_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
}
else {
item = PyTuple_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
}
return v;
}
// internal function to set a float matrix[16]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// matrix - float[16] which will be set by values from objMat
static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
int i, len;
PyObject* seq;
seq = PySequence_Fast(objMat, "expected a sequence");
if (seq)
{
len = PySequence_Size(objMat);
if (len == 16) {
for (i = 0; i < len; i++) {
matrix[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// internal function to set a float vector[3]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// vector - float[3] which will be set by values from objMat
static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) {
int i, len;
PyObject* seq = 0;
if (objVec == NULL)
return 0;
seq = PySequence_Fast(objVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(objVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
int i, len;
PyObject* seq;
if (obVec == NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(obVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) {
int i, len;
PyObject* seq;
if (obVec == NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 4) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 0;
}
// Step through one timestep of the simulation
static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) {
if (0 == sm) {
@@ -371,19 +488,6 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
return PyLong_FromLong(bodyIndex);
}
static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
double v = 0.0;
PyObject* item;
if (PyList_Check(seq)) {
item = PyList_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
} else {
item = PyTuple_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
}
return v;
}
@@ -768,11 +872,68 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args)
return Py_None;
}
static int pybullet_internalGetBaseVelocity(
int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3]) {
baseLinearVelocity[0] = 0.;
baseLinearVelocity[1] = 0.;
baseLinearVelocity[2] = 0.;
baseAngularVelocity[0] = 0.;
baseAngularVelocity[1] = 0.;
baseAngularVelocity[2] = 0.;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return 0;
}
{
{
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
const int status_type = b3GetStatusType(status_handle);
const double* actualStateQdot;
// const double* jointReactionForces[];
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
PyErr_SetString(SpamError, "getBaseVelocity failed.");
return 0;
}
b3GetStatusActualState(
status_handle, 0 /* body_unique_id */,
0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
0 /*root_local_inertial_frame*/, 0,
&actualStateQdot, 0 /* joint_reaction_forces */);
// printf("joint reaction forces=");
// for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) {
// printf("%f ", jointReactionForces[i]);
// }
// now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2]
// and orientation x,y,z,w =
// actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6]
baseLinearVelocity[0] = actualStateQdot[0];
baseLinearVelocity[1] = actualStateQdot[1];
baseLinearVelocity[2] = actualStateQdot[2];
baseAngularVelocity[0] = actualStateQdot[3];
baseAngularVelocity[1] = actualStateQdot[4];
baseAngularVelocity[2] = actualStateQdot[5];
}
}
return 1;
}
// Internal function used to get the base position and orientation
// Orientation is returned in quaternions
static int pybullet_internalGetBasePositionAndOrientation(
int bodyIndex, double basePosition[3], double baseOrientation[3]) {
int bodyIndex, double basePosition[3], double baseOrientation[4]) {
basePosition[0] = 0.;
basePosition[1] = 0.;
basePosition[2] = 0.;
@@ -855,8 +1016,7 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
if (0 == pybullet_internalGetBasePositionAndOrientation(
bodyIndex, basePosition, baseOrientation)) {
PyErr_SetString(SpamError,
"GetBasePositionAndOrientation failed (#joints/links "
"exceeds maximum?).");
"GetBasePositionAndOrientation failed.");
return NULL;
}
@@ -891,6 +1051,62 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
}
}
static PyObject* pybullet_getBaseVelocity(PyObject* self,
PyObject* args) {
int bodyIndex = -1;
double baseLinearVelocity[3];
double baseAngularVelocity[3];
PyObject* pylistLinVel=0;
PyObject* pylistAngVel=0;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTuple(args, "i", &bodyIndex)) {
PyErr_SetString(SpamError, "Expected a body index (integer).");
return NULL;
}
if (0 == pybullet_internalGetBaseVelocity(
bodyIndex, baseLinearVelocity, baseAngularVelocity)) {
PyErr_SetString(SpamError,
"getBaseVelocity failed.");
return NULL;
}
{
PyObject* item;
int i;
int num = 3;
pylistLinVel = PyTuple_New(num);
for (i = 0; i < num; i++) {
item = PyFloat_FromDouble(baseLinearVelocity[i]);
PyTuple_SetItem(pylistLinVel, i, item);
}
}
{
PyObject* item;
int i;
int num = 3;
pylistAngVel = PyTuple_New(num);
for (i = 0; i < num; i++) {
item = PyFloat_FromDouble(baseAngularVelocity[i]);
PyTuple_SetItem(pylistAngVel, i, item);
}
}
{
PyObject* pylist;
pylist = PyTuple_New(2);
PyTuple_SetItem(pylist, 0, pylistLinVel);
PyTuple_SetItem(pylist, 1, pylistAngVel);
return pylist;
}
}
static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args)
{
if (0 == sm) {
@@ -1035,6 +1251,66 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) {
return NULL;
}
static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject *keywds)
{
static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity", NULL };
if (0 == sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int bodyIndex=0;
PyObject* linVelObj=0;
PyObject* angVelObj=0;
double linVel[3] = { 0, 0, 0 };
double angVel[3] = { 0, 0, 0 };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OO", kwlist, &bodyIndex, &linVelObj, &angVelObj))
{
return NULL;
}
if (linVelObj || angVelObj)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
if (linVelObj)
{
pybullet_internalSetVectord(linVelObj, linVel);
b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVel);
}
if (angVelObj)
{
pybullet_internalSetVectord(angVelObj, angVel);
b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVel);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_INCREF(Py_None);
return Py_None;
}
else
{
PyErr_SetString(SpamError, "expected at least linearVelocity and/or angularVelocity.");
return NULL;
}
}
PyErr_SetString(SpamError, "error in resetJointState.");
return NULL;
}
// Reset the position and orientation of the base/root link, position [x,y,z]
// and orientation quaternion [x,y,z,w]
static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self,
@@ -1366,105 +1642,6 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) {
return Py_None;
}
// internal function to set a float matrix[16]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// matrix - float[16] which will be set by values from objMat
static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
int i, len;
PyObject* seq;
seq = PySequence_Fast(objMat, "expected a sequence");
if (seq)
{
len = PySequence_Size(objMat);
if (len == 16) {
for (i = 0; i < len; i++) {
matrix[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// internal function to set a float vector[3]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// vector - float[3] which will be set by values from objMat
static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) {
int i, len;
PyObject* seq=0;
if (objVec==NULL)
return 0;
seq = PySequence_Fast(objVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(objVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
int i, len;
PyObject* seq;
if (obVec==NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(obVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) {
int i, len;
PyObject* seq;
if (obVec==NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 4) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 0;
}
static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds)
{
@@ -1501,7 +1678,7 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
res = pybullet_internalSetVectord(textPositionObj,posXYZ);
if (!res)
{
PyErr_SetString(SpamError, "Error converting lineFrom[3]");
PyErr_SetString(SpamError, "Error converting textPositionObj[3]");
return NULL;
}
@@ -1510,7 +1687,7 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
res = pybullet_internalSetVectord(textColorRGBObj,colorRGB);
if (!res)
{
PyErr_SetString(SpamError, "Error converting lineTo[3]");
PyErr_SetString(SpamError, "Error converting textColorRGBObj[3]");
return NULL;
}
}
@@ -1647,8 +1824,45 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject *keywds)
{
PyObject* objectColorRGBObj = 0;
double objectColorRGB[3];
int objectUniqueId = -1;
int linkIndex = -2;
static char *kwlist[] = { "objectUniqueId", "linkIndex","objectDebugColorRGB", NULL };
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|O", kwlist,
&objectUniqueId, &linkIndex, &objectColorRGBObj))
return NULL;
if (objectColorRGBObj)
{
if (pybullet_internalSetVectord(objectColorRGBObj, objectColorRGB))
{
b3SharedMemoryCommandHandle commandHandle = b3InitDebugDrawingCommand(sm);
b3SetDebugObjectColor(commandHandle, objectUniqueId, linkIndex, objectColorRGB);
b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
}
}
else
{
b3SharedMemoryCommandHandle commandHandle = b3InitDebugDrawingCommand(sm);
b3RemoveDebugObjectColor(commandHandle, objectUniqueId, linkIndex);
b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
{
@@ -1848,23 +2062,22 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5-6-7 double m_positionOnAInWS[3];//contact point location on object A,
5 double m_positionOnAInWS[3];//contact point location on object A,
in world space coordinates
8-9-10 double m_positionOnBInWS[3];//contact point location on object
6 double m_positionOnBInWS[3];//contact point location on object
A, in world space coordinates
11-12-13 double m_contactNormalOnBInWS[3];//the separating contact
7 double m_contactNormalOnBInWS[3];//the separating contact
normal, pointing from object B towards object A
14 double m_contactDistance;//negative number is penetration, positive
8 double m_contactDistance;//negative number is penetration, positive
is distance.
15 double m_normalForce;
9 double m_normalForce;
*/
int i;
PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints);
for (i = 0; i < contactPointPtr->m_numContactPoints; i++) {
PyObject* contactObList = PyTuple_New(16); // see above 16 fields
PyObject* contactObList = PyTuple_New(10); // see above 10 fields
PyObject* item;
item =
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags);
@@ -1881,42 +2094,61 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin
item =
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB);
PyTuple_SetItem(contactObList, 4, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]);
PyTuple_SetItem(contactObList, 5, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]);
PyTuple_SetItem(contactObList, 6, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]);
PyTuple_SetItem(contactObList, 7, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]);
PyTuple_SetItem(contactObList, 8, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]);
PyTuple_SetItem(contactObList, 9, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]);
PyTuple_SetItem(contactObList, 10, item);
{
PyObject* posAObj = PyTuple_New(3);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]);
PyTuple_SetItem(contactObList, 11, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]);
PyTuple_SetItem(contactObList, 12, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]);
PyTuple_SetItem(contactObList, 13, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]);
PyTuple_SetItem(posAObj, 0, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]);
PyTuple_SetItem(posAObj, 1, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]);
PyTuple_SetItem(posAObj, 2, item);
PyTuple_SetItem(contactObList, 5, posAObj);
}
{
PyObject* posBObj = PyTuple_New(3);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]);
PyTuple_SetItem(posBObj, 0, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]);
PyTuple_SetItem(posBObj, 1, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]);
PyTuple_SetItem(posBObj, 2, item);
PyTuple_SetItem(contactObList, 6, posBObj);
}
{
PyObject* normalOnB = PyTuple_New(3);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]);
PyTuple_SetItem(normalOnB, 0, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]);
PyTuple_SetItem(normalOnB, 1, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]);
PyTuple_SetItem(normalOnB, 2, item);
PyTuple_SetItem(contactObList, 7, normalOnB);
}
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactDistance);
PyTuple_SetItem(contactObList, 14, item);
PyTuple_SetItem(contactObList, 8, item);
item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_normalForce);
PyTuple_SetItem(contactObList, 15, item);
PyTuple_SetItem(contactObList, 9, item);
PyTuple_SetItem(pyResultList, i, contactObList);
}
@@ -1982,6 +2214,9 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
int size = PySequence_Size(args);
int bodyUniqueIdA = -1;
int bodyUniqueIdB = -1;
int linkIndexA = -2;
int linkIndexB = -2;
double distanceThreshold = 0.f;
b3SharedMemoryCommandHandle commandHandle;
@@ -1991,15 +2226,15 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
PyObject* pyResultList = 0;
static char *kwlist[] = { "bodyA", "bodyB", "distance", NULL };
static char *kwlist[] = { "bodyA", "bodyB", "distance", "linkIndexA","linkIndexB",NULL };
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|ii", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB))
return NULL;
@@ -2007,7 +2242,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
b3SetClosestDistanceThreshold(commandHandle, distanceThreshold);
if (linkIndexA >= -1)
{
b3SetClosestDistanceFilterLinkA(commandHandle, linkIndexA);
}
if (linkIndexB >= -1)
{
b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
@@ -3241,11 +3483,6 @@ static PyMethodDef SpamMethods[] = {
"Set the amount of time to proceed at each call to stepSimulation. (unit "
"is seconds, typically range is 0.01 or 0.001)"},
{"setTimeStep", pybullet_setTimeStep, METH_VARARGS,
"Set the amount of time to proceed at each call to stepSimulation."
" (unit is seconds, typically range is 0.01 or 0.001)"},
{"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS,
"Set the amount of contact penetration Error Recovery Paramater "
"(ERP) in each time step. \
@@ -3306,6 +3543,19 @@ static PyMethodDef SpamMethods[] = {
"instantaneously, not through physics simulation. (x,y,z) position vector "
"and (x,y,z,w) quaternion orientation."},
{ "getBaseVelocity", pybullet_getBaseVelocity,
METH_VARARGS,
"Get the linear and angular velocity of the base of the object "
" in world space coordinates. "
"(x,y,z) linear velocity vector and (x,y,z) angular velocity vector." },
{ "resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS,
"Reset the linear and/or angular velocity of the base of the object "
" in world space coordinates. "
"linearVelocity (x,y,z) and angularVelocity (x,y,z)." },
{"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
"Get the number of joints for an object."},
@@ -3404,6 +3654,10 @@ static PyMethodDef SpamMethods[] = {
"remove all user debug draw items"
},
{ "setDebugObjectColor", (PyCFunction)pybullet_setDebugObjectColor, METH_VARARGS | METH_KEYWORDS,
"Override the wireframe debug drawing color for a particular object unique id / link index."
"If you ommit the color, the custom color will be removed."
},
{"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,

3
examples/pybullet/testrender.py
View File

@@ -19,6 +19,7 @@ pixelHeight = 240
nearPlane = 0.01
farPlane = 1000
lightDirection = [0,1,0]
lightColor = [1,1,1]#optional argument
fov = 60
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
@@ -28,7 +29,7 @@ for pitch in range (0,360,10) :
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection)
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor)
w=img_arr[0]
h=img_arr[1]
rgb=img_arr[2]