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

regular OR wireframe rendering, not both

Browse Source 
add option to perform filtering of 'getClosestPoints' using linkA/linkB.
don't use 'realtimesimulation' as default
add/remove debug items within same thread
pybullet, report contact points and normal as [x,y,z] triplet/vector, not 3 scalars
separate 'getClosestPointsAlgorithm': box-box doesn't report closest points with positive distance, and the query shouldn't impact regular 'closesst points'
This commit is contained in:
erwincoumans
2016-11-19 17:13:56 -08:00
parent 936a104fb2
commit 9ee1c4ec24
19 changed files with 417 additions and 222 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1504,6 +1504,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;
}
@@ -1516,6 +1518,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;

4
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -123,12 +123,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);

36
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),
@@ -2769,6 +2769,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;
@@ -2783,15 +2789,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);
}
}
}
}
@@ -2811,15 +2823,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);
}
}
}
}

223
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -126,10 +126,6 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIHelperRemoveAllGraphicsInstances,
eGUIHelperCopyCameraImageData,
eGUIHelperAutogenerateGraphicsObjects,
eGUIUserDebugAddText,
eGUIUserDebugAddLine,
eGUIUserDebugRemoveItem,
eGUIUserDebugRemoveAllItems,
};
#include <stdio.h>
@@ -701,43 +697,36 @@ public:
m_tmpText.m_textColorRGB[2] = textColorRGB[2];
m_cs->lock();
m_cs->setSharedParam(1, eGUIUserDebugAddText);
m_userDebugText.push_back(m_tmpText);
m_cs->unlock();
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
{
b3Clock::usleep(150);
}
return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId;
}
btAlignedObjectArray<UserDebugDrawLine> m_userDebugLines;
UserDebugDrawLine m_tmpLine;
virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime )
{
m_tmpLine.m_lifeTime = lifeTime;
m_tmpLine.m_lineWidth = lineWidth;
m_tmpLine.m_itemUniqueId = m_uidGenerator++;
m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0];
m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1];
m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2];
UserDebugDrawLine tmpLine;
m_tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0];
m_tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1];
m_tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2];
tmpLine.m_lifeTime = lifeTime;
tmpLine.m_lineWidth = lineWidth;
tmpLine.m_itemUniqueId = m_uidGenerator++;
tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0];
tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1];
tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2];
tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0];
tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1];
tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2];
m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0];
m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1];
m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2];
tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0];
tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1];
tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2];
m_cs->lock();
m_cs->setSharedParam(1, eGUIUserDebugAddLine);
m_userDebugLines.push_back(tmpLine);
m_cs->unlock();
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
{
b3Clock::usleep(150);
}
return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId;
}
@@ -747,23 +736,38 @@ public:
{
m_removeDebugItemUid = debugItemUniqueId;
m_cs->lock();
m_cs->setSharedParam(1, eGUIUserDebugRemoveItem);
m_cs->unlock();
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
for (int i = 0; i<m_userDebugLines.size(); i++)
{
b3Clock::usleep(150);
if (m_userDebugLines[i].m_itemUniqueId == m_removeDebugItemUid)
{
m_userDebugLines.swap(i, m_userDebugLines.size() - 1);
m_userDebugLines.pop_back();
break;
}
}
for (int i = 0; i<m_userDebugText.size(); i++)
{
if (m_userDebugText[i].m_itemUniqueId == m_removeDebugItemUid)
{
m_userDebugText.swap(i, m_userDebugText.size() - 1);
m_userDebugText.pop_back();
break;
}
}
m_cs->unlock();
}
virtual void removeAllUserDebugItems( )
{
m_cs->lock();
m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems);
m_userDebugLines.clear();
m_userDebugText.clear();
m_uidGenerator = 0;
m_cs->unlock();
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
{
b3Clock::usleep(150);
}
}
@@ -825,6 +829,7 @@ public:
virtual bool wantsTermination();
virtual bool isConnected();
virtual void renderScene();
void drawUserDebugLines();
virtual void exitPhysics();
virtual void physicsDebugDraw(int debugFlags);
@@ -1247,60 +1252,6 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
break;
}
case eGUIUserDebugAddText:
{
m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIUserDebugAddLine:
{
m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIUserDebugRemoveItem:
{
for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
{
if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
{
m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
m_multiThreadedHelper->m_userDebugLines.pop_back();
break;
}
}
for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
{
if (m_multiThreadedHelper->m_userDebugText[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
{
m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
m_multiThreadedHelper->m_userDebugText.pop_back();
break;
}
}
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIUserDebugRemoveAllItems:
{
m_multiThreadedHelper->m_userDebugLines.clear();
m_multiThreadedHelper->m_userDebugText.clear();
m_multiThreadedHelper->m_uidGenerator = 0;
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperIdle:
{
break;
@@ -1353,7 +1304,53 @@ 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)
{
m_args[0].m_cs->lock();
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);
}
m_args[0].m_cs->unlock();
}
}
void PhysicsServerExample::renderScene()
{
@@ -1369,48 +1366,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 +1381,7 @@ void PhysicsServerExample::renderScene()
static int count = 0;
count++;
#if 0
if (0 == (count & 1))
{
btScalar curTime = m_clock.getTimeSeconds();
@@ -1444,6 +1402,7 @@ void PhysicsServerExample::renderScene()
worseFps = 1000000;
}
}
#endif
#ifdef BT_ENABLE_VR
if ((gInternalSimFlags&2 ) && m_tinyVrGui==0)
@@ -1544,6 +1503,8 @@ void PhysicsServerExample::renderScene()
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
{
drawUserDebugLines();
///debug rendering
m_physicsServer.physicsDebugDraw(debugDrawFlags);

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -154,6 +154,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
@@ -161,6 +163,8 @@ struct RequestContactDataArgs
int m_startingContactPointIndex;
int m_objectAIndexFilter;
int m_objectBIndexFilter;
int m_linkIndexAIndexFilter;
int m_linkIndexBIndexFilter;
double m_closestDistanceThreshold;
int m_mode;
};