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

fix in box-box contact generation: choose better contact point location (along contact normal) when objects are swapped.

Browse Source 
move debugDrawWorld to btCollisionWorld.
improved CollisionInterfaceDemo, show how to perform a closest point query for objects that are not in the collision world.
removed a bit of garbage from the debug drawer
This commit is contained in:
erwin.coumans
2009-12-18 00:54:52 +00:00
parent 6425af9a6c
commit fcd2b93a22
7 changed files with 619 additions and 501 deletions
Download Patch File Download Diff File Expand all files Collapse all files

345
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -35,20 +35,8 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
//for debug rendering
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/CollisionShapes/btConeShape.h"
#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
#include "LinearMath/btIDebugDraw.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletDynamics/Dynamics/btActionInterface.h"
@@ -127,24 +115,8 @@ void btDiscreteDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("debugDrawWorld");
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
{
int numManifolds = getDispatcher()->getNumManifolds();
btVector3 color(0,0,0);
for (int i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
btCollisionWorld::debugDrawWorld();
int numContacts = contactManifold->getNumContacts();
for (int j=0;j<numContacts;j++)
{
btManifoldPoint& cp = contactManifold->getContactPoint(j);
getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
}
}
}
bool drawConstraints = false;
if (getDebugDrawer())
{
@@ -169,42 +141,6 @@ void btDiscreteDynamicsWorld::debugDrawWorld()
{
int i;
for ( i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
{
btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
switch(colObj->getActivationState())
{
case ACTIVE_TAG:
color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
case ISLAND_SLEEPING:
color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
case WANTS_DEACTIVATION:
color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
case DISABLE_DEACTIVATION:
color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
case DISABLE_SIMULATION:
color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
default:
{
color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
}
};
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
}
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btVector3 minAabb,maxAabb;
btVector3 colorvec(1,0,0);
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
}
}
if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
{
for (i=0;i<m_actions.size();i++)
@@ -929,283 +865,6 @@ void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
{
btIDebugDraw* m_debugDrawer;
btVector3 m_color;
btTransform m_worldTrans;
public:
DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
m_debugDrawer(debugDrawer),
m_color(color),
m_worldTrans(worldTrans)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{
processTriangle(triangle,partId,triangleIndex);
}
virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
{
(void)partId;
(void)triangleIndex;
btVector3 wv0,wv1,wv2;
wv0 = m_worldTrans*triangle[0];
wv1 = m_worldTrans*triangle[1];
wv2 = m_worldTrans*triangle[2];
m_debugDrawer->drawLine(wv0,wv1,m_color);
m_debugDrawer->drawLine(wv1,wv2,m_color);
m_debugDrawer->drawLine(wv2,wv0,m_color);
}
};
void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
{
btVector3 start = transform.getOrigin();
const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
// XY
getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
// XZ
getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
// YZ
getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
}
void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
{
// Draw a small simplex at the center of the object
{
btVector3 start = worldTransform.getOrigin();
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
}
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* colShape = compoundShape->getChildShape(i);
debugDrawObject(worldTransform*childTrans,colShape,color);
}
} else
{
switch (shape->getShapeType())
{
case SPHERE_SHAPE_PROXYTYPE:
{
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
debugDrawSphere(radius, worldTransform, color);
break;
}
case MULTI_SPHERE_SHAPE_PROXYTYPE:
{
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
btTransform childTransform;
childTransform.setIdentity();
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
{
childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
debugDrawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
}
break;
}
case CAPSULE_SHAPE_PROXYTYPE:
{
const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
btScalar radius = capsuleShape->getRadius();
btScalar halfHeight = capsuleShape->getHalfHeight();
int upAxis = capsuleShape->getUpAxis();
btVector3 capStart(0.f,0.f,0.f);
capStart[upAxis] = -halfHeight;
btVector3 capEnd(0.f,0.f,0.f);
capEnd[upAxis] = halfHeight;
// Draw the ends
{
btTransform childTransform = worldTransform;
childTransform.getOrigin() = worldTransform * capStart;
debugDrawSphere(radius, childTransform, color);
}
{
btTransform childTransform = worldTransform;
childTransform.getOrigin() = worldTransform * capEnd;
debugDrawSphere(radius, childTransform, color);
}
// Draw some additional lines
btVector3 start = worldTransform.getOrigin();
capStart[(upAxis+1)%3] = radius;
capEnd[(upAxis+1)%3] = radius;
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
capStart[(upAxis+1)%3] = -radius;
capEnd[(upAxis+1)%3] = -radius;
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
capStart[(upAxis+1)%3] = 0.f;
capEnd[(upAxis+1)%3] = 0.f;
capStart[(upAxis+2)%3] = radius;
capEnd[(upAxis+2)%3] = radius;
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
capStart[(upAxis+2)%3] = -radius;
capEnd[(upAxis+2)%3] = -radius;
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
break;
}
case CONE_SHAPE_PROXYTYPE:
{
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
btScalar height = coneShape->getHeight();//+coneShape->getMargin();
btVector3 start = worldTransform.getOrigin();
int upAxis= coneShape->getConeUpIndex();
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = height * btScalar(0.5);
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
btVector3 offset2Radius(0,0,0);
offset2Radius[(upAxis+2)%3] = radius;
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
break;
}
case CYLINDER_SHAPE_PROXYTYPE:
{
const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
int upAxis = cylinder->getUpAxis();
btScalar radius = cylinder->getRadius();
btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
btVector3 start = worldTransform.getOrigin();
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = halfHeight;
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
break;
}
case STATIC_PLANE_PROXYTYPE:
{
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
btScalar planeConst = staticPlaneShape->getPlaneConstant();
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
btVector3 planeOrigin = planeNormal * planeConst;
btVector3 vec0,vec1;
btPlaneSpace1(planeNormal,vec0,vec1);
btScalar vecLen = 100.f;
btVector3 pt0 = planeOrigin + vec0*vecLen;
btVector3 pt1 = planeOrigin - vec0*vecLen;
btVector3 pt2 = planeOrigin + vec1*vecLen;
btVector3 pt3 = planeOrigin - vec1*vecLen;
getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
break;
}
default:
{
if (shape->isConcave())
{
btConcaveShape* concaveMesh = (btConcaveShape*) shape;
///@todo pass camera, for some culling? no -> we are not a graphics lib
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
}
if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{
btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
//todo: pass camera for some culling
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
//DebugDrawcallback drawCallback;
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
}
/// for polyhedral shapes
if (shape->isPolyhedral())
{
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
int i;
for (i=0;i<polyshape->getNumEdges();i++)
{
btVector3 a,b;
polyshape->getEdge(i,a,b);
btVector3 wa = worldTransform * a;
btVector3 wb = worldTransform * b;
getDebugDrawer()->drawLine(wa,wb,color);
}
}
}
}
}
}
void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;