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

131
Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp
View File

@@ -17,6 +17,7 @@ subject to the following restrictions:
/// ///
/// CollisionInterfaceDemo shows high level usage of the Collision Detection. /// CollisionInterfaceDemo shows high level usage of the Collision Detection.
/// ///
#define TEST_NOT_ADDING_OBJECTS_TO_WORLD
#include "GL_Simplex1to4.h" #include "GL_Simplex1to4.h"
@@ -57,7 +58,7 @@ int main(int argc,char** argv)
void CollisionInterfaceDemo::initPhysics() void CollisionInterfaceDemo::initPhysics()
{ {
//m_debugMode |= btIDebugDraw::DBG_DrawWireframe; m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
btMatrix3x3 basisA; btMatrix3x3 basisA;
basisA.setIdentity(); basisA.setIdentity();
@@ -69,7 +70,10 @@ void CollisionInterfaceDemo::initPhysics()
objects[1].getWorldTransform().setBasis(basisB); objects[1].getWorldTransform().setBasis(basisB);
btBoxShape* boxA = new btBoxShape(btVector3(1,1,1)); btBoxShape* boxA = new btBoxShape(btVector3(1,1,1));
boxA->setMargin(0.f);
btBoxShape* boxB = new btBoxShape(btVector3(0.5,0.5,0.5)); btBoxShape* boxB = new btBoxShape(btVector3(0.5,0.5,0.5));
boxB->setMargin(0.f);
//ConvexHullShape hullA(points0,3); //ConvexHullShape hullA(points0,3);
//hullA.setLocalScaling(btVector3(3,3,3)); //hullA.setLocalScaling(btVector3(3,3,3));
//ConvexHullShape hullB(points1,4); //ConvexHullShape hullB(points1,4);
@@ -89,9 +93,13 @@ void CollisionInterfaceDemo::initPhysics()
//SimpleBroadphase* broadphase = new btSimpleBroadphase; //SimpleBroadphase* broadphase = new btSimpleBroadphase;
collisionWorld = new btCollisionWorld(dispatcher,broadphase,collisionConfiguration); collisionWorld = new btCollisionWorld(dispatcher,broadphase,collisionConfiguration);
collisionWorld->setDebugDrawer(&debugDrawer);
#ifdef TEST_NOT_ADDING_OBJECTS_TO_WORLD
collisionWorld->addCollisionObject(&objects[0]); collisionWorld->addCollisionObject(&objects[0]);
collisionWorld->addCollisionObject(&objects[1]); collisionWorld->addCollisionObject(&objects[1]);
#endif //TEST_NOT_ADDING_OBJECTS_TO_WORLD
} }
@@ -115,14 +123,33 @@ void CollisionInterfaceDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING); glDisable(GL_LIGHTING);
btScalar m[16];
btVector3 worldBoundsMin,worldBoundsMax;
collisionWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
int i;
for (i=0;i<numObjects;i++)
{
objects[i].getWorldTransform().getOpenGLMatrix( m );
m_shapeDrawer->drawOpenGL(m,objects[i].getCollisionShape(),btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
collisionWorld->getDispatchInfo().m_debugDraw = &debugDrawer; collisionWorld->getDispatchInfo().m_debugDraw = &debugDrawer;
if (collisionWorld) if (collisionWorld)
collisionWorld->performDiscreteCollisionDetection(); collisionWorld->performDiscreteCollisionDetection();
int i;
///one way to draw all the contact points is iterating over contact manifolds / points:
#ifndef TEST_NOT_ADDING_OBJECTS_TO_WORLD
collisionWorld->debugDrawWorld();
///one way to draw all the contact points is iterating over contact manifolds in the dispatcher:
int numManifolds = collisionWorld->getDispatcher()->getNumManifolds(); int numManifolds = collisionWorld->getDispatcher()->getNumManifolds();
for (i=0;i<numManifolds;i++) for (i=0;i<numManifolds;i++)
@@ -137,7 +164,7 @@ void CollisionInterfaceDemo::displayCallback(void) {
btManifoldPoint& pt = contactManifold->getContactPoint(j); btManifoldPoint& pt = contactManifold->getContactPoint(j);
glBegin(GL_LINES); glBegin(GL_LINES);
glColor3f(1, 0, 1); glColor3f(0, 0, 0);
btVector3 ptA = pt.getPositionWorldOnA(); btVector3 ptA = pt.getPositionWorldOnA();
btVector3 ptB = pt.getPositionWorldOnB(); btVector3 ptB = pt.getPositionWorldOnB();
@@ -150,33 +177,88 @@ void CollisionInterfaceDemo::displayCallback(void) {
//you can un-comment out this line, and then all points are removed //you can un-comment out this line, and then all points are removed
//contactManifold->clearManifold(); //contactManifold->clearManifold();
} }
#else
glDisable(GL_TEXTURE_2D);
for (i=0;i<numObjects;i++)
{
collisionWorld->debugDrawObject(objects[i].getWorldTransform(),objects[i].getCollisionShape(), btVector3(1,1,0));
}
//another way is to directly query the dispatcher for both objects. The objects don't need to be inserted into the world
btCollisionAlgorithm* algo = collisionWorld->getDispatcher()->findAlgorithm(&objects[0],&objects[1]);
btManifoldResult contactPointResult(&objects[0],&objects[1]);
algo->processCollision(&objects[0],&objects[1],collisionWorld->getDispatchInfo(),&contactPointResult);
btManifoldArray manifoldArray;
algo->getAllContactManifolds(manifoldArray);
int numManifolds = manifoldArray.size();
for (i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = manifoldArray[i];
btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
glDisable(GL_DEPTH_TEST);
int numContacts = contactManifold->getNumContacts();
bool swap = obA == &objects[0];
for (int j=0;j<numContacts;j++)
{
btManifoldPoint& pt = contactManifold->getContactPoint(j);
glBegin(GL_LINES);
glColor3f(0, 0, 0);
btVector3 ptA = swap ?pt.getPositionWorldOnA():pt.getPositionWorldOnB();
btVector3 ptB = swap ? pt.getPositionWorldOnB():pt.getPositionWorldOnA();
glVertex3d(ptA.x(),ptA.y(),ptA.z());
glVertex3d(ptB.x(),ptB.y(),ptB.z());
glEnd();
}
//you can un-comment out this line, and then all points are removed
//contactManifold->clearManifold();
}
#endif
//GL_ShapeDrawer::drawCoordSystem(); //GL_ShapeDrawer::drawCoordSystem();
btScalar m[16];
btVector3 worldBoundsMin,worldBoundsMax; btQuaternion qA = objects[0].getWorldTransform().getRotation();
collisionWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax); btQuaternion qB = objects[1].getWorldTransform().getRotation();
for (i=0;i<numObjects;i++) if (!m_idle)
{ {
objects[i].getWorldTransform().getOpenGLMatrix( m );
m_shapeDrawer->drawOpenGL(m,objects[i].getCollisionShape(),btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax); btScalar timeInSeconds = getDeltaTimeMicroseconds()/1000.f;
btQuaternion orn;
objects[0].getWorldTransform().getBasis().getEulerYPR(yaw,pitch,roll);
pitch += 0.00005f*timeInSeconds;
yaw += 0.0001f*timeInSeconds;
objects[0].getWorldTransform().getBasis().setEulerYPR(yaw,pitch,roll);
orn.setEuler(yaw,pitch,roll);
objects[1].getWorldTransform().setOrigin(objects[1].getWorldTransform().getOrigin()+btVector3(0,-0.00001*timeInSeconds,0));
//objects[0].getWorldTransform().setRotation(orn);
} }
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
objects[1].getWorldTransform().setOrigin(objects[1].getWorldTransform().getOrigin()+btVector3(0,-0.01,0));
objects[0].getWorldTransform().setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush(); glFlush();
glutSwapBuffers(); glutSwapBuffers();
} }
@@ -184,7 +266,14 @@ void CollisionInterfaceDemo::displayCallback(void) {
void CollisionInterfaceDemo::clientResetScene() void CollisionInterfaceDemo::clientResetScene()
{ {
objects[0].getWorldTransform().setOrigin(btVector3(0.0f,3.f,0.f)); objects[0].getWorldTransform().setOrigin(btVector3(0.0f,3.f,0.f));
objects[1].getWorldTransform().setOrigin(btVector3(0.0f,9.f,0.f));
btQuaternion rotA(0.739,-0.204,0.587,0.257);
rotA.normalize();
objects[0].getWorldTransform().setRotation(rotA);
objects[1].getWorldTransform().setOrigin(btVector3(0.0f,4.248f,0.f));
} }

38
Demos/OpenGL/GL_ShapeDrawer.cpp
View File

@@ -601,17 +601,25 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
7,2,3, 7,2,3,
7,6,2}; 7,6,2};
static btVector3 vertices[8]={ btVector3(1,1,1),btVector3(-1,1,1), btVector3(1,-1,1), btVector3(-1,-1,1), btVector3(1,1,-1), btVector3(-1,1,-1), btVector3(1,-1,-1), btVector3(-1,-1,-1)}; btVector3 vertices[8]={
btVector3(halfExtent[0],halfExtent[1],halfExtent[2]),
btVector3(-halfExtent[0],halfExtent[1],halfExtent[2]),
btVector3(halfExtent[0],-halfExtent[1],halfExtent[2]),
btVector3(-halfExtent[0],-halfExtent[1],halfExtent[2]),
btVector3(halfExtent[0],halfExtent[1],-halfExtent[2]),
btVector3(-halfExtent[0],halfExtent[1],-halfExtent[2]),
btVector3(halfExtent[0],-halfExtent[1],-halfExtent[2]),
btVector3(-halfExtent[0],-halfExtent[1],-halfExtent[2])};
#if 1
glBegin (GL_TRIANGLES); glBegin (GL_TRIANGLES);
int si=36; int si=36;
for (int i=0;i<si;i+=3) for (int i=0;i<si;i+=3)
{ {
btVector3 v1 = vertices[indices[i]]*halfExtent; const btVector3& v1 = vertices[indices[i]];;
btVector3 v2 = vertices[indices[i+1]]*halfExtent; const btVector3& v2 = vertices[indices[i+1]];
btVector3 v3 = vertices[indices[i+2]]*halfExtent; const btVector3& v3 = vertices[indices[i+2]];
btVector3 normal = (v3-v1).cross(v2-v1); btVector3 normal = (v3-v1).cross(v2-v1);
normal.normalize (); normal.normalize ();
glNormal3f(normal.getX(),normal.getY(),normal.getZ()); glNormal3f(normal.getX(),normal.getY(),normal.getZ());
glVertex3f (v1.x(), v1.y(), v1.z()); glVertex3f (v1.x(), v1.y(), v1.z());
glVertex3f (v2.x(), v2.y(), v2.z()); glVertex3f (v2.x(), v2.y(), v2.z());
@@ -619,6 +627,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
} }
glEnd(); glEnd();
#endif
useWireframeFallback = false; useWireframeFallback = false;
break; break;
@@ -812,8 +821,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
{ {
glRasterPos3f(0.0, 0.0, 0.0);
//btDrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->getExtraDebugInfo());
glColor3f(1.f, 1.f, 1.f); glColor3f(1.f, 1.f, 1.f);
int i; int i;
@@ -821,7 +828,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
{ {
btVector3 vtx; btVector3 vtx;
polyshape->getVertex(i,vtx); polyshape->getVertex(i,vtx);
glRasterPos3f(vtx.x(), vtx.y(), vtx.z());
char buf[12]; char buf[12];
sprintf(buf," %d",i); sprintf(buf," %d",i);
//btDrawString(BMF_GetFont(BMF_kHelvetica10),buf); //btDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
@@ -834,7 +840,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
polyshape->getPlane(normal,vtx,i); polyshape->getPlane(normal,vtx,i);
btScalar d = vtx.dot(normal); btScalar d = vtx.dot(normal);
glRasterPos3f(normal.x()*d, normal.y()*d, normal.z()*d);
char buf[12]; char buf[12];
sprintf(buf," plane %d",i); sprintf(buf," plane %d",i);
//btDrawString(BMF_GetFont(BMF_kHelvetica10),buf); //btDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
@@ -878,21 +883,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
glDisable(GL_DEPTH_TEST);
glRasterPos3f(0,0,0);//mvtx.x(), vtx.y(), vtx.z());
if (debugMode&btIDebugDraw::DBG_DrawText)
{
GLDebugDrawString(0,0,shape->getName());
}
if (debugMode& btIDebugDraw::DBG_DrawFeaturesText)
{
//btDrawString(BMF_GetFont(BMF_kHelvetica10),shape->getExtraDebugInfo());
}
glEnable(GL_DEPTH_TEST);
// glPopMatrix();
if(m_textureenabled) glDisable(GL_TEXTURE_2D);
} }
glPopMatrix(); glPopMatrix();

35
src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
View File

@@ -1,4 +1,3 @@
/* /*
* Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
* Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
@@ -424,6 +423,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
output.addContactPoint(-normal,pointInWorld,-*depth); output.addContactPoint(-normal,pointInWorld,-*depth);
#else #else
output.addContactPoint(-normal,pb,-*depth); output.addContactPoint(-normal,pb,-*depth);
#endif // #endif //
*return_code = code; *return_code = code;
} }
@@ -593,21 +593,30 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
if (maxc < 1) maxc = 1; if (maxc < 1) maxc = 1;
if (cnum <= maxc) { if (cnum <= maxc) {
if (code<4)
{
// we have less contacts than we need, so we use them all // we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++) { for (j=0; j < cnum; j++)
{
//AddContactPoint...
//dContactGeom *con = CONTACT(contact,skip*j);
//for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
//con->depth = dep[j];
btVector3 pointInWorld; btVector3 pointInWorld;
for (i=0; i<3; i++) for (i=0; i<3; i++)
pointInWorld[i] = point[j*3+i] + pa[i]; pointInWorld[i] = point[j*3+i] + pa[i];
output.addContactPoint(-normal,pointInWorld,-dep[j]); output.addContactPoint(-normal,pointInWorld,-dep[j]);
} }
} else
{
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++)
{
btVector3 pointInWorld;
for (i=0; i<3; i++)
pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
//pointInWorld[i] = point[j*3+i] + pa[i];
output.addContactPoint(-normal,pointInWorld,-dep[j]);
}
}
} }
else { else {
// we have more contacts than are wanted, some of them must be culled. // we have more contacts than are wanted, some of them must be culled.
@@ -632,7 +641,13 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
btVector3 posInWorld; btVector3 posInWorld;
for (i=0; i<3; i++) for (i=0; i<3; i++)
posInWorld[i] = point[iret[j]*3+i] + pa[i]; posInWorld[i] = point[iret[j]*3+i] + pa[i];
output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); if (code<4)
{
output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
} else
{
output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
}
} }
cnum = maxc; cnum = maxc;
} }

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

@@ -42,6 +42,24 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
///for debug drawing
//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"
btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
:m_dispatcher1(dispatcher), :m_dispatcher1(dispatcher),
m_broadphasePairCache(pairCache), m_broadphasePairCache(pairCache),
@@ -92,27 +110,27 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
btAssert(collisionObject); btAssert(collisionObject);
//check that the object isn't already added //check that the object isn't already added
btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
m_collisionObjects.push_back(collisionObject); m_collisionObjects.push_back(collisionObject);
//calculate new AABB //calculate new AABB
btTransform trans = collisionObject->getWorldTransform(); btTransform trans = collisionObject->getWorldTransform();
btVector3 minAabb; btVector3 minAabb;
btVector3 maxAabb; btVector3 maxAabb;
collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb); collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
int type = collisionObject->getCollisionShape()->getShapeType(); int type = collisionObject->getCollisionShape()->getShapeType();
collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
minAabb, minAabb,
maxAabb, maxAabb,
type, type,
collisionObject, collisionObject,
collisionFilterGroup, collisionFilterGroup,
collisionFilterMask, collisionFilterMask,
m_dispatcher1,0 m_dispatcher1,0
)) ; )) ;
@@ -228,10 +246,10 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
btCollisionObject* collisionObject, btCollisionObject* collisionObject,
const btCollisionShape* collisionShape, const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform, const btTransform& colObjWorldTransform,
RayResultCallback& resultCallback) RayResultCallback& resultCallback)
{ {
btSphereShape pointShape(btScalar(0.0)); btSphereShape pointShape(btScalar(0.0));
pointShape.setMargin(0.f); pointShape.setMargin(0.f);
@@ -239,7 +257,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
if (collisionShape->isConvex()) if (collisionShape->isConvex())
{ {
// BT_PROFILE("rayTestConvex"); // BT_PROFILE("rayTestConvex");
btConvexCast::CastResult castResult; btConvexCast::CastResult castResult;
castResult.m_fraction = resultCallback.m_closestHitFraction; castResult.m_fraction = resultCallback.m_closestHitFraction;
@@ -268,10 +286,10 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
castResult.m_normal.normalize(); castResult.m_normal.normalize();
btCollisionWorld::LocalRayResult localRayResult btCollisionWorld::LocalRayResult localRayResult
( (
collisionObject, collisionObject,
0, 0,
castResult.m_normal, castResult.m_normal,
castResult.m_fraction castResult.m_fraction
); );
bool normalInWorldSpace = true; bool normalInWorldSpace = true;
@@ -283,7 +301,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
} else { } else {
if (collisionShape->isConcave()) if (collisionShape->isConcave())
{ {
// BT_PROFILE("rayTestConcave"); // BT_PROFILE("rayTestConcave");
if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
{ {
///optimized version for btBvhTriangleMeshShape ///optimized version for btBvhTriangleMeshShape
@@ -299,18 +317,18 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
btCollisionObject* m_collisionObject; btCollisionObject* m_collisionObject;
btTriangleMeshShape* m_triangleMesh; btTriangleMeshShape* m_triangleMesh;
btTransform m_colObjWorldTransform; btTransform m_colObjWorldTransform;
BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform): btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform):
//@BP Mod //@BP Mod
btTriangleRaycastCallback(from,to, resultCallback->m_flags), btTriangleRaycastCallback(from,to, resultCallback->m_flags),
m_resultCallback(resultCallback), m_resultCallback(resultCallback),
m_collisionObject(collisionObject), m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh), m_triangleMesh(triangleMesh),
m_colObjWorldTransform(colObjWorldTransform) m_colObjWorldTransform(colObjWorldTransform)
{ {
} }
virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
@@ -319,10 +337,10 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
shapeInfo.m_shapePart = partId; shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex; shapeInfo.m_triangleIndex = triangleIndex;
btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
btCollisionWorld::LocalRayResult rayResult btCollisionWorld::LocalRayResult rayResult
(m_collisionObject, (m_collisionObject,
&shapeInfo, &shapeInfo,
hitNormalWorld, hitNormalWorld,
hitFraction); hitFraction);
@@ -354,18 +372,18 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
btCollisionObject* m_collisionObject; btCollisionObject* m_collisionObject;
btConcaveShape* m_triangleMesh; btConcaveShape* m_triangleMesh;
btTransform m_colObjWorldTransform; btTransform m_colObjWorldTransform;
BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform): btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
//@BP Mod //@BP Mod
btTriangleRaycastCallback(from,to, resultCallback->m_flags), btTriangleRaycastCallback(from,to, resultCallback->m_flags),
m_resultCallback(resultCallback), m_resultCallback(resultCallback),
m_collisionObject(collisionObject), m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh), m_triangleMesh(triangleMesh),
m_colObjWorldTransform(colObjWorldTransform) m_colObjWorldTransform(colObjWorldTransform)
{ {
} }
virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
@@ -374,10 +392,10 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
shapeInfo.m_shapePart = partId; shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex; shapeInfo.m_triangleIndex = triangleIndex;
btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
btCollisionWorld::LocalRayResult rayResult btCollisionWorld::LocalRayResult rayResult
(m_collisionObject, (m_collisionObject,
&shapeInfo, &shapeInfo,
hitNormalWorld, hitNormalWorld,
hitFraction); hitFraction);
@@ -400,7 +418,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
} }
} else { } else {
// BT_PROFILE("rayTestCompound"); // BT_PROFILE("rayTestCompound");
///@todo: use AABB tree or other BVH acceleration structure, see btDbvt ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
if (collisionShape->isCompound()) if (collisionShape->isCompound())
{ {
@@ -428,10 +446,10 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
} }
void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
btCollisionObject* collisionObject, btCollisionObject* collisionObject,
const btCollisionShape* collisionShape, const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform, const btTransform& colObjWorldTransform,
ConvexResultCallback& resultCallback, btScalar allowedPenetration) ConvexResultCallback& resultCallback, btScalar allowedPenetration)
{ {
if (collisionShape->isConvex()) if (collisionShape->isConvex())
{ {
@@ -461,13 +479,13 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
{ {
castResult.m_normal.normalize(); castResult.m_normal.normalize();
btCollisionWorld::LocalConvexResult localConvexResult btCollisionWorld::LocalConvexResult localConvexResult
( (
collisionObject, collisionObject,
0, 0,
castResult.m_normal, castResult.m_normal,
castResult.m_hitPoint, castResult.m_hitPoint,
castResult.m_fraction castResult.m_fraction
); );
bool normalInWorldSpace = true; bool normalInWorldSpace = true;
resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
@@ -497,12 +515,12 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
m_resultCallback(resultCallback), m_resultCallback(resultCallback),
m_collisionObject(collisionObject), m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh) m_triangleMesh(triangleMesh)
{ {
} }
virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
@@ -514,7 +532,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
{ {
btCollisionWorld::LocalConvexResult convexResult btCollisionWorld::LocalConvexResult convexResult
(m_collisionObject, (m_collisionObject,
&shapeInfo, &shapeInfo,
hitNormalLocal, hitNormalLocal,
hitPointLocal, hitPointLocal,
@@ -554,12 +572,12 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
m_resultCallback(resultCallback), m_resultCallback(resultCallback),
m_collisionObject(collisionObject), m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh) m_triangleMesh(triangleMesh)
{ {
} }
virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
@@ -571,7 +589,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
{ {
btCollisionWorld::LocalConvexResult convexResult btCollisionWorld::LocalConvexResult convexResult
(m_collisionObject, (m_collisionObject,
&shapeInfo, &shapeInfo,
hitNormalLocal, hitNormalLocal,
hitPointLocal, hitPointLocal,
@@ -641,10 +659,10 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
btCollisionWorld::RayResultCallback& m_resultCallback; btCollisionWorld::RayResultCallback& m_resultCallback;
btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback) btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
:m_rayFromWorld(rayFromWorld), :m_rayFromWorld(rayFromWorld),
m_rayToWorld(rayToWorld), m_rayToWorld(rayToWorld),
m_world(world), m_world(world),
m_resultCallback(resultCallback) m_resultCallback(resultCallback)
{ {
m_rayFromTrans.setIdentity(); m_rayFromTrans.setIdentity();
m_rayFromTrans.setOrigin(m_rayFromWorld); m_rayFromTrans.setOrigin(m_rayFromWorld);
@@ -697,9 +715,9 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
{ {
m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
collisionObject, collisionObject,
collisionObject->getCollisionShape(), collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(), collisionObject->getWorldTransform(),
m_resultCallback); m_resultCallback);
} }
} }
return true; return true;
@@ -771,11 +789,11 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback
if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans, m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
collisionObject, collisionObject,
collisionObject->getCollisionShape(), collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(), collisionObject->getWorldTransform(),
m_resultCallback, m_resultCallback,
m_allowedCcdPenetration); m_allowedCcdPenetration);
} }
return true; return true;
@@ -835,12 +853,355 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT
{ {
objectQuerySingle(castShape, convexFromTrans,convexToTrans, objectQuerySingle(castShape, convexFromTrans,convexToTrans,
collisionObject, collisionObject,
collisionObject->getCollisionShape(), collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(), collisionObject->getWorldTransform(),
resultCallback, resultCallback,
allowedCcdPenetration); allowedCcdPenetration);
} }
} }
} }
#endif //USE_BRUTEFORCE_RAYBROADPHASE #endif //USE_BRUTEFORCE_RAYBROADPHASE
} }
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 btCollisionWorld::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 btCollisionWorld::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 btCollisionWorld::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());
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);
}
}
}
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
{
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);
}
}
}
}

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

@@ -150,6 +150,12 @@ public:
return m_debugDrawer; return m_debugDrawer;
} }
virtual void debugDrawWorld();
virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
///LocalShapeInfo gives extra information for complex shapes ///LocalShapeInfo gives extra information for complex shapes
///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart ///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart

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/btGeneric6DofConstraint.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.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 "LinearMath/btIDebugDraw.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletDynamics/Dynamics/btActionInterface.h" #include "BulletDynamics/Dynamics/btActionInterface.h"
@@ -127,24 +115,8 @@ void btDiscreteDynamicsWorld::debugDrawWorld()
{ {
BT_PROFILE("debugDrawWorld"); BT_PROFILE("debugDrawWorld");
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) btCollisionWorld::debugDrawWorld();
{
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());
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; bool drawConstraints = false;
if (getDebugDrawer()) if (getDebugDrawer())
{ {
@@ -169,42 +141,6 @@ void btDiscreteDynamicsWorld::debugDrawWorld()
{ {
int i; 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()) if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
{ {
for (i=0;i<m_actions.size();i++) 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) void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{ {
bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;

2
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -77,7 +77,6 @@ protected:
virtual void saveKinematicState(btScalar timeStep); virtual void saveKinematicState(btScalar timeStep);
void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
public: public:
@@ -135,7 +134,6 @@ public:
///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
virtual void removeCollisionObject(btCollisionObject* collisionObject); virtual void removeCollisionObject(btCollisionObject* collisionObject);
void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
void debugDrawConstraint(btTypedConstraint* constraint); void debugDrawConstraint(btTypedConstraint* constraint);