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

2
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -1167,7 +1167,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
} }
BT_PROFILE("Render Scene"); BT_PROFILE("Render Scene");
sCurrentDemo->renderScene(); sCurrentDemo->renderScene();
} } else
{ {
B3_PROFILE("physicsDebugDraw"); B3_PROFILE("physicsDebugDraw");
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); 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; 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) int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -686,6 +720,8 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
return 0; return 0;
} }
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition) int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -1195,6 +1231,46 @@ int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle)
return status->m_userDebugDrawArgs.m_debugItemUniqueId; 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. ///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient) b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient)
@@ -1533,6 +1609,8 @@ b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClient
command->m_requestContactPointArguments.m_startingContactPointIndex = 0; command->m_requestContactPointArguments.m_startingContactPointIndex = 0;
command->m_requestContactPointArguments.m_objectAIndexFilter = -1; command->m_requestContactPointArguments.m_objectAIndexFilter = -1;
command->m_requestContactPointArguments.m_objectBIndexFilter = -1; command->m_requestContactPointArguments.m_objectBIndexFilter = -1;
command->m_requestContactPointArguments.m_linkIndexAIndexFilter = -2;
command->m_requestContactPointArguments.m_linkIndexBIndexFilter = -2;
command->m_updateFlags = 0; command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
} }
@@ -1545,6 +1623,37 @@ void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int body
command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA; 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) void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; 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 b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime);
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId); b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId);
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient); 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. ///All debug items unique Ids are positive: a negative unique Id means failure.
int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle);
///request an image from a simulated camera, using a software renderer. ///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
@@ -126,12 +132,16 @@ void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle comm
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient); b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); 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); void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
///compute the closest points between two bodies ///compute the closest points between two bodies
b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient); b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient);
void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance); void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo); void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
@@ -248,6 +258,9 @@ int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, do
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); 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 b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); 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_kukaGripperMultiBody(0),
m_kukaGripperRevolute1(0), m_kukaGripperRevolute1(0),
m_kukaGripperRevolute2(0), m_kukaGripperRevolute2(0),
m_allowRealTimeSimulation(true), m_allowRealTimeSimulation(false),
m_huskyId(-1), m_huskyId(-1),
m_KukaId(-1), m_KukaId(-1),
m_sphereId(-1), m_sphereId(-1),
@@ -2352,6 +2352,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (body && body->m_multiBody) if (body && body->m_multiBody)
{ {
btMultiBody* mb = 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) if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
{ {
btVector3 zero(0,0,0); 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[1] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]);
mb->setBaseVel(zero); mb->setBaseVel(baseLinVel);
mb->setBasePos(btVector3( mb->setBasePos(btVector3(
clientCmd.m_initPoseArgs.m_initialStateQ[0], clientCmd.m_initPoseArgs.m_initialStateQ[0],
clientCmd.m_initPoseArgs.m_initialStateQ[1], 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[5] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
mb->setBaseOmega(btVector3(0,0,0)); mb->setBaseOmega(baseAngVel);
btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3], btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3],
clientCmd.m_initPoseArgs.m_initialStateQ[4], clientCmd.m_initPoseArgs.m_initialStateQ[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5], clientCmd.m_initPoseArgs.m_initialStateQ[5],
@@ -2784,6 +2806,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter; int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter;
int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter; 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*> setA;
btAlignedObjectArray<btCollisionObject*> setB; btAlignedObjectArray<btCollisionObject*> setB;
btAlignedObjectArray<int> setALinkIndex; btAlignedObjectArray<int> setALinkIndex;
@@ -2798,15 +2826,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
if (bodyA->m_multiBody->getBaseCollider()) if (bodyA->m_multiBody->getBaseCollider())
{ {
setA.push_back(bodyA->m_multiBody->getBaseCollider()); if (!hasLinkIndexAFilter || (linkIndexA == -1))
setALinkIndex.push_back(-1); {
setA.push_back(bodyA->m_multiBody->getBaseCollider());
setALinkIndex.push_back(-1);
}
} }
for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++) for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++)
{ {
if (bodyA->m_multiBody->getLink(i).m_collider) if (bodyA->m_multiBody->getLink(i).m_collider)
{ {
setA.push_back(bodyA->m_multiBody->getLink(i).m_collider); if (!hasLinkIndexAFilter || (linkIndexA == i))
setALinkIndex.push_back(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()) if (bodyB->m_multiBody->getBaseCollider())
{ {
setB.push_back(bodyB->m_multiBody->getBaseCollider()); if (!hasLinkIndexBFilter || (linkIndexB == -1))
setBLinkIndex.push_back(-1); {
setB.push_back(bodyB->m_multiBody->getBaseCollider());
setBLinkIndex.push_back(-1);
}
} }
for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++) for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++)
{ {
if (bodyB->m_multiBody->getLink(i).m_collider) if (bodyB->m_multiBody->getLink(i).m_collider)
{ {
setB.push_back(bodyB->m_multiBody->getLink(i).m_collider); if (!hasLinkIndexBFilter || (linkIndexB ==i))
setBLinkIndex.push_back(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++) for( int i=0;i<numRb;i++)
{ {
btCollisionObject* colObj = importer->getRigidBodyByIndex(i); btCollisionObject* colObj = importer->getRigidBodyByIndex(i);
if (colObj) if (colObj)
{ {
btRigidBody* rb = btRigidBody::upcast(colObj); btRigidBody* rb = btRigidBody::upcast(colObj);
@@ -3591,7 +3630,54 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
hasStatus = true; 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) 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 wantsTermination();
virtual bool isConnected(); virtual bool isConnected();
virtual void renderScene(); virtual void renderScene();
void drawUserDebugLines();
virtual void exitPhysics(); virtual void exitPhysics();
virtual void physicsDebugDraw(int debugFlags); virtual void physicsDebugDraw(int debugFlags);
@@ -1353,7 +1354,51 @@ extern int gHuskyId;
extern btTransform huskyTr; 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() void PhysicsServerExample::renderScene()
{ {
@@ -1369,48 +1414,8 @@ void PhysicsServerExample::renderScene()
B3_PROFILE("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) if (gEnableRealTimeSimVR)
{ {
@@ -1424,6 +1429,7 @@ void PhysicsServerExample::renderScene()
static int count = 0; static int count = 0;
count++; count++;
#if 0
if (0 == (count & 1)) if (0 == (count & 1))
{ {
btScalar curTime = m_clock.getTimeSeconds(); btScalar curTime = m_clock.getTimeSeconds();
@@ -1444,6 +1450,7 @@ void PhysicsServerExample::renderScene()
worseFps = 1000000; worseFps = 1000000;
} }
} }
#endif
#ifdef BT_ENABLE_VR #ifdef BT_ENABLE_VR
if ((gInternalSimFlags&2 ) && m_tinyVrGui==0) 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)); tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
b3Scalar dt = 0.01; b3Scalar dt = 0.01;
m_tinyVrGui->clearTextArea(); m_tinyVrGui->clearTextArea();
static char line0[1024];
static char line1[1024];
m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255); m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255); m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);
@@ -1544,6 +1553,8 @@ void PhysicsServerExample::renderScene()
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags) void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
{ {
drawUserDebugLines();
///debug rendering ///debug rendering
m_physicsServer.physicsDebugDraw(debugDrawFlags); 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_POSITION=1,
INIT_POSE_HAS_INITIAL_ORIENTATION=2, 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_bodyUniqueId;
int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM]; int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQ[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_QUERY_MODE=1,
CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD=2, 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 struct RequestContactDataArgs
@@ -167,6 +173,8 @@ struct RequestContactDataArgs
int m_startingContactPointIndex; int m_startingContactPointIndex;
int m_objectAIndexFilter; int m_objectAIndexFilter;
int m_objectBIndexFilter; int m_objectBIndexFilter;
int m_linkIndexAIndexFilter;
int m_linkIndexBIndexFilter;
double m_closestDistanceThreshold; double m_closestDistanceThreshold;
int m_mode; int m_mode;
}; };
@@ -526,7 +534,10 @@ enum EnumUserDebugDrawFlags
USER_DEBUG_HAS_LINE=1, USER_DEBUG_HAS_LINE=1,
USER_DEBUG_HAS_TEXT=2, USER_DEBUG_HAS_TEXT=2,
USER_DEBUG_REMOVE_ONE_ITEM=4, 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 struct UserDebugDrawArgs
@@ -543,6 +554,10 @@ struct UserDebugDrawArgs
double m_textPositionXYZ[3]; double m_textPositionXYZ[3];
double m_textColorRGB[3]; double m_textColorRGB[3];
double m_textSize; 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 center(0,0,0);
Vec3f up(0,0,1); Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0); m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1); m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity(); m_modelMatrix = Matrix::identity();
@@ -193,6 +194,7 @@ m_objectIndex(objectIndex)
Vec3f center(0,0,0); Vec3f center(0,0,0);
Vec3f up(0,0,1); Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0); m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1); m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity(); m_modelMatrix = Matrix::identity();

580
examples/pybullet/pybullet.c
View File

@@ -30,6 +30,123 @@ enum eCONNECT_METHOD {
static PyObject* SpamError; static PyObject* SpamError;
static b3PhysicsClientHandle sm = 0; 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 // Step through one timestep of the simulation
static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) { static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) {
if (0 == sm) { if (0 == sm) {
@@ -371,19 +488,6 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
return PyLong_FromLong(bodyIndex); 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; 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 // Internal function used to get the base position and orientation
// Orientation is returned in quaternions // Orientation is returned in quaternions
static int pybullet_internalGetBasePositionAndOrientation( static int pybullet_internalGetBasePositionAndOrientation(
int bodyIndex, double basePosition[3], double baseOrientation[3]) { int bodyIndex, double basePosition[3], double baseOrientation[4]) {
basePosition[0] = 0.; basePosition[0] = 0.;
basePosition[1] = 0.; basePosition[1] = 0.;
basePosition[2] = 0.; basePosition[2] = 0.;
@@ -855,8 +1016,7 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
if (0 == pybullet_internalGetBasePositionAndOrientation( if (0 == pybullet_internalGetBasePositionAndOrientation(
bodyIndex, basePosition, baseOrientation)) { bodyIndex, basePosition, baseOrientation)) {
PyErr_SetString(SpamError, PyErr_SetString(SpamError,
"GetBasePositionAndOrientation failed (#joints/links " "GetBasePositionAndOrientation failed.");
"exceeds maximum?).");
return NULL; 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) static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args)
{ {
if (0 == sm) { if (0 == sm) {
@@ -1035,6 +1251,66 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) {
return NULL; 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] // Reset the position and orientation of the base/root link, position [x,y,z]
// and orientation quaternion [x,y,z,w] // and orientation quaternion [x,y,z,w]
static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self,
@@ -1366,105 +1642,6 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) {
return Py_None; 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) 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); res = pybullet_internalSetVectord(textPositionObj,posXYZ);
if (!res) if (!res)
{ {
PyErr_SetString(SpamError, "Error converting lineFrom[3]"); PyErr_SetString(SpamError, "Error converting textPositionObj[3]");
return NULL; return NULL;
} }
@@ -1510,7 +1687,7 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
res = pybullet_internalSetVectord(textColorRGBObj,colorRGB); res = pybullet_internalSetVectord(textColorRGBObj,colorRGB);
if (!res) if (!res)
{ {
PyErr_SetString(SpamError, "Error converting lineTo[3]"); PyErr_SetString(SpamError, "Error converting textColorRGBObj[3]");
return NULL; return NULL;
} }
} }
@@ -1647,8 +1824,45 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args
Py_INCREF(Py_None); Py_INCREF(Py_None);
return 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) static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
{ {
@@ -1848,23 +2062,22 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin
2 int m_bodyUniqueIdB; 2 int m_bodyUniqueIdB;
3 int m_linkIndexA; 3 int m_linkIndexA;
4 int m_linkIndexB; 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 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 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 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. is distance.
9 double m_normalForce;
15 double m_normalForce;
*/ */
int i; int i;
PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints); PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints);
for (i = 0; i < contactPointPtr->m_numContactPoints; i++) { 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; PyObject* item;
item = item =
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags); PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags);
@@ -1881,42 +2094,61 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin
item = item =
PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB); PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB);
PyTuple_SetItem(contactObList, 4, item); 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]); PyObject* posAObj = PyTuple_New(3);
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);
item = PyFloat_FromDouble( item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]); contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]);
PyTuple_SetItem(contactObList, 11, item); PyTuple_SetItem(posAObj, 0, item);
item = PyFloat_FromDouble( item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]); contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]);
PyTuple_SetItem(contactObList, 12, item); PyTuple_SetItem(posAObj, 1, item);
item = PyFloat_FromDouble( item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]); contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]);
PyTuple_SetItem(contactObList, 13, item); 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( item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_contactDistance); contactPointPtr->m_contactPointData[i].m_contactDistance);
PyTuple_SetItem(contactObList, 14, item); PyTuple_SetItem(contactObList, 8, item);
item = PyFloat_FromDouble( item = PyFloat_FromDouble(
contactPointPtr->m_contactPointData[i].m_normalForce); contactPointPtr->m_contactPointData[i].m_normalForce);
PyTuple_SetItem(contactObList, 15, item); PyTuple_SetItem(contactObList, 9, item);
PyTuple_SetItem(pyResultList, i, contactObList); PyTuple_SetItem(pyResultList, i, contactObList);
} }
@@ -1982,6 +2214,9 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
int size = PySequence_Size(args); int size = PySequence_Size(args);
int bodyUniqueIdA = -1; int bodyUniqueIdA = -1;
int bodyUniqueIdB = -1; int bodyUniqueIdB = -1;
int linkIndexA = -2;
int linkIndexB = -2;
double distanceThreshold = 0.f; double distanceThreshold = 0.f;
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
@@ -1991,15 +2226,15 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
PyObject* pyResultList = 0; PyObject* pyResultList = 0;
static char *kwlist[] = { "bodyA", "bodyB", "distance", NULL }; static char *kwlist[] = { "bodyA", "bodyB", "distance", "linkIndexA","linkIndexB",NULL };
if (0 == sm) { if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server."); PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL; return NULL;
} }
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid", kwlist, if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|ii", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold)) &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB))
return NULL; return NULL;
@@ -2007,7 +2242,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA); b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB); b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
b3SetClosestDistanceThreshold(commandHandle, distanceThreshold); b3SetClosestDistanceThreshold(commandHandle, distanceThreshold);
if (linkIndexA >= -1)
{
b3SetClosestDistanceFilterLinkA(commandHandle, linkIndexA);
}
if (linkIndexB >= -1)
{
b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
@@ -3241,11 +3483,6 @@ static PyMethodDef SpamMethods[] = {
"Set the amount of time to proceed at each call to stepSimulation. (unit " "Set the amount of time to proceed at each call to stepSimulation. (unit "
"is seconds, typically range is 0.01 or 0.001)"}, "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, {"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS,
"Set the amount of contact penetration Error Recovery Paramater " "Set the amount of contact penetration Error Recovery Paramater "
"(ERP) in each time step. \ "(ERP) in each time step. \
@@ -3306,6 +3543,19 @@ static PyMethodDef SpamMethods[] = {
"instantaneously, not through physics simulation. (x,y,z) position vector " "instantaneously, not through physics simulation. (x,y,z) position vector "
"and (x,y,z,w) quaternion orientation."}, "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, {"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
"Get the number of joints for an object."}, "Get the number of joints for an object."},
@@ -3404,6 +3654,10 @@ static PyMethodDef SpamMethods[] = {
"remove all user debug draw items" "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, {"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,

3
examples/pybullet/testrender.py
View File

@@ -19,6 +19,7 @@ pixelHeight = 240
nearPlane = 0.01 nearPlane = 0.01
farPlane = 1000 farPlane = 1000
lightDirection = [0,1,0] lightDirection = [0,1,0]
lightColor = [1,1,1]#optional argument
fov = 60 fov = 60
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight) #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) viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight; aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); 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] w=img_arr[0]
h=img_arr[1] h=img_arr[1]
rgb=img_arr[2] rgb=img_arr[2]

8
src/BulletCollision/BroadphaseCollision/btDispatcher.h
View File

@@ -64,6 +64,12 @@ struct btDispatcherInfo
btScalar m_convexConservativeDistanceThreshold; btScalar m_convexConservativeDistanceThreshold;
}; };
enum ebtDispatcherQueryType
{
BT_CONTACT_POINT_ALGORITHMS = 1,
BT_CLOSEST_POINT_ALGORITHMS = 2
};
///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs. ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.
///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic). ///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic).
class btDispatcher class btDispatcher
@@ -73,7 +79,7 @@ class btDispatcher
public: public:
virtual ~btDispatcher() ; virtual ~btDispatcher() ;
virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold=0) = 0; virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0;
virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0; virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0;

2
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
View File

@@ -100,7 +100,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
btScalar radiusWithThreshold = radius + contactBreakingThreshold; btScalar radiusWithThreshold = radius + contactBreakingThreshold;
btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
normal.normalize(); normal.safeNormalize();
btVector3 p1ToCentre = sphereCenter - vertices[0]; btVector3 p1ToCentre = sphereCenter - vertices[0];
btScalar distanceFromPlane = p1ToCentre.dot(normal); btScalar distanceFromPlane = p1ToCentre.dot(normal);

3
src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
View File

@@ -40,6 +40,9 @@ public:
virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0;
}; };
#endif //BT_COLLISION_CONFIGURATION #endif //BT_COLLISION_CONFIGURATION

28
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
View File

@@ -50,8 +50,10 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH
{ {
for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
{ {
m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j); m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
btAssert(m_doubleDispatch[i][j]); btAssert(m_doubleDispatchContactPoints[i][j]);
m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j);
} }
} }
@@ -61,7 +63,12 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH
void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
{ {
m_doubleDispatch[proxyType0][proxyType1] = createFunc; m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc;
}
void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
{
m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc;
} }
btCollisionDispatcher::~btCollisionDispatcher() btCollisionDispatcher::~btCollisionDispatcher()
@@ -138,14 +145,23 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold)
btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType)
{ {
btCollisionAlgorithmConstructionInfo ci; btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher1 = this; ci.m_dispatcher1 = this;
ci.m_manifold = sharedManifold; ci.m_manifold = sharedManifold;
btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap); btCollisionAlgorithm* algo = 0;
if (algoType == BT_CONTACT_POINT_ALGORITHMS)
{
algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
}
else
{
algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
}
return algo; return algo;
} }
@@ -262,7 +278,7 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair,
//dispatcher will keep algorithms persistent in the collision pair //dispatcher will keep algorithms persistent in the collision pair
if (!collisionPair.m_algorithm) if (!collisionPair.m_algorithm)
{ {
collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap); collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap,0, BT_CONTACT_POINT_ALGORITHMS);
} }
if (collisionPair.m_algorithm) if (collisionPair.m_algorithm)

8
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
View File

@@ -57,7 +57,9 @@ protected:
btPoolAllocator* m_persistentManifoldPoolAllocator; btPoolAllocator* m_persistentManifoldPoolAllocator;
btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
btCollisionConfiguration* m_collisionConfiguration; btCollisionConfiguration* m_collisionConfiguration;
@@ -84,6 +86,8 @@ public:
///registerCollisionCreateFunc allows registration of custom/alternative collision create functions ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc);
int getNumManifolds() const int getNumManifolds() const
{ {
return int( m_manifoldsPtr.size()); return int( m_manifoldsPtr.size());
@@ -115,7 +119,7 @@ public:
virtual void clearManifold(btPersistentManifold* manifold); virtual void clearManifold(btPersistentManifold* manifold);
btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold = 0); btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType);
virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1); virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1);

24
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@@ -122,6 +122,7 @@ protected:
///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation. ///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation.
int m_updateRevision; int m_updateRevision;
btVector3 m_customDebugColorRGB;
public: public:
@@ -136,7 +137,8 @@ public:
CF_CHARACTER_OBJECT = 16, CF_CHARACTER_OBJECT = 16,
CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
CF_HAS_CONTACT_STIFFNESS_DAMPING = 128 CF_HAS_CONTACT_STIFFNESS_DAMPING = 128,
CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
}; };
enum CollisionObjectTypes enum CollisionObjectTypes
@@ -556,6 +558,26 @@ public:
return m_updateRevision; return m_updateRevision;
} }
void setCustomDebugColor(const btVector3& colorRGB)
{
m_customDebugColorRGB = colorRGB;
m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
}
void removeCustomDebugColor()
{
m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
}
bool getCustomDebugColor(btVector3& colorRGB) const
{
bool hasCustomColor = (0!=(m_collisionFlags&CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR));
if (hasCustomColor)
{
colorRGB = m_customDebugColorRGB;
}
return hasCustomColor;
}
inline bool checkCollideWith(const btCollisionObject* co) const inline bool checkCollideWith(const btCollisionObject* co) const
{ {

6
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -1231,7 +1231,7 @@ struct btSingleContactCallback : public btBroadphaseAabbCallback
btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1); btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1);
btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1); btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1);
btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1); btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1,0, BT_CLOSEST_POINT_ALGORITHMS);
if (algorithm) if (algorithm)
{ {
btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback); btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback);
@@ -1267,7 +1267,7 @@ void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionOb
btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1); btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1);
btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1); btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1);
btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB); btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB, 0, BT_CLOSEST_POINT_ALGORITHMS);
if (algorithm) if (algorithm)
{ {
btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback); btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
@@ -1572,6 +1572,8 @@ void btCollisionWorld::debugDrawWorld()
} }
}; };
colObj->getCustomDebugColor(color);
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
} }
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))

42
src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
View File

@@ -65,7 +65,13 @@ void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionO
const btCollisionShape* childShape = compoundShape->getChildShape(i); const btCollisionShape* childShape = compoundShape->getChildShape(i);
btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully) btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully)
m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold); m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact;
btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints;
} }
} }
} }
@@ -128,8 +134,14 @@ public:
btTransform newChildWorldTrans = orgTrans*childTrans ; btTransform newChildWorldTrans = orgTrans*childTrans ;
//perform an AABB check first //perform an AABB check first
btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; btVector3 aabbMin0,aabbMax0;
childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= extendAabb;
aabbMax0 += extendAabb;
btVector3 aabbMin1, aabbMax1;
m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
if (gCompoundChildShapePairCallback) if (gCompoundChildShapePairCallback)
@@ -142,12 +154,22 @@ public:
{ {
btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
btCollisionAlgorithm* algo = 0;
if (m_resultOut->m_closestPointDistanceThreshold > 0)
//the contactpoint is still projected back using the original inverted worldtrans {
if (!m_childCollisionAlgorithms[index]) algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold); }
else
{
//the contactpoint is still projected back using the original inverted worldtrans
if (!m_childCollisionAlgorithms[index])
{
m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
}
algo = m_childCollisionAlgorithms[index];
}
const btCollisionObjectWrapper* tmpWrap = 0; const btCollisionObjectWrapper* tmpWrap = 0;
@@ -164,8 +186,7 @@ public:
m_resultOut->setShapeIdentifiersB(-1,index); m_resultOut->setShapeIdentifiersB(-1,index);
} }
algo->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
#if 0 #if 0
if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
@@ -271,6 +292,9 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
btTransform otherInCompoundSpace; btTransform otherInCompoundSpace;
otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform(); otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
localAabbMin -= extraExtends;
localAabbMax += extraExtends;
const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
//process all children, that overlap with the given AABB bounds //process all children, that overlap with the given AABB bounds

29
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
View File

@@ -164,9 +164,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
aabbMin0 -= thresholdVec; aabbMin0 -= thresholdVec;
aabbMin1 -= thresholdVec;
aabbMax0 += thresholdVec; aabbMax0 += thresholdVec;
aabbMax1 += thresholdVec;
if (gCompoundCompoundChildShapePairCallback) if (gCompoundCompoundChildShapePairCallback)
{ {
@@ -183,17 +181,24 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
btCollisionAlgorithm* colAlgo = 0; btCollisionAlgorithm* colAlgo = 0;
if (m_resultOut->m_closestPointDistanceThreshold > 0)
{
colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
}
else
{
if (pair)
{
colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
if (pair) }
{ else
colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; {
colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
} else pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1);
{ btAssert(pair);
colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); pair->m_userPointer = colAlgo;
pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); }
btAssert(pair);
pair->m_userPointer = colAlgo;
} }
btAssert(colAlgo); btAssert(colAlgo);

15
src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
View File

@@ -118,8 +118,16 @@ partId, int triangleIndex)
btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform? btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr); btCollisionAlgorithm* colAlgo = 0;
if (m_resultOut->m_closestPointDistanceThreshold > 0)
{
colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
}
else
{
colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS);
}
const btCollisionObjectWrapper* tmpWrap = 0; const btCollisionObjectWrapper* tmpWrap = 0;
if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
@@ -170,7 +178,8 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape()); const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
btScalar extraMargin = collisionMarginTriangle; btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold;
btVector3 extra(extraMargin,extraMargin,extraMargin); btVector3 extra(extraMargin,extraMargin,extraMargin);
m_aabbMax += extra; m_aabbMax += extra;

80
src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
View File

@@ -198,6 +198,86 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
} }
btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1)
{
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_sphereSphereCF;
}
#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
{
return m_sphereBoxCF;
}
if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_boxSphereCF;
}
#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE))
{
return m_sphereTriangleCF;
}
if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
{
return m_triangleSphereCF;
}
if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
{
return m_convexPlaneCF;
}
if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE))
{
return m_planeConvexCF;
}
if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
{
return m_convexConvexCreateFunc;
}
if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
{
return m_convexConcaveCreateFunc;
}
if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
{
return m_swappedConvexConcaveCreateFunc;
}
if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1))
{
return m_compoundCompoundCreateFunc;
}
if (btBroadphaseProxy::isCompound(proxyType0))
{
return m_compoundCreateFunc;
}
else
{
if (btBroadphaseProxy::isCompound(proxyType1))
{
return m_swappedCompoundCreateFunc;
}
}
//failed to find an algorithm
return m_emptyCreateFunc;
}
btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
{ {

2
src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
View File

@@ -103,6 +103,8 @@ public:
virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1);
///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm. ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.
///By default, this feature is disabled for best performance. ///By default, this feature is disabled for best performance.
///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature. ///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature.

2
src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
View File

@@ -56,7 +56,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObje
/// report a contact. internally this will be kept persistent, and contact reduction is done /// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr); resultOut->setPersistentManifold(m_manifoldPtr);
SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold);
btDiscreteCollisionDetectorInterface::ClosestPointInput input; btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds

2
src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h
View File

@@ -122,7 +122,7 @@ protected:
checkManifold(body0Wrap,body1Wrap); checkManifold(body0Wrap,body1Wrap);
btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm( btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(
body0Wrap,body1Wrap,getLastManifold()); body0Wrap,body1Wrap,getLastManifold(), BT_CONTACT_POINT_ALGORITHMS);
return convex_algorithm ; return convex_algorithm ;
} }

7
src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp
View File

@@ -120,8 +120,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId,
btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
//btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//?? //btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//??
btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex); btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);
ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS;
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr); btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr);
colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->~btCollisionAlgorithm(); colAlgo->~btCollisionAlgorithm();
@@ -164,7 +164,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId,
btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//?? btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//??
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr); ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS;
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr);
colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->~btCollisionAlgorithm(); colAlgo->~btCollisionAlgorithm();