fix in box-box contact generation: choose better contact point location (along contact normal) when objects are swapped.
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
@@ -17,6 +17,7 @@ subject to the following restrictions:
|
||||
///
|
||||
/// CollisionInterfaceDemo shows high level usage of the Collision Detection.
|
||||
///
|
||||
#define TEST_NOT_ADDING_OBJECTS_TO_WORLD
|
||||
|
||||
#include "GL_Simplex1to4.h"
|
||||
|
||||
@@ -57,7 +58,7 @@ int main(int argc,char** argv)
|
||||
void CollisionInterfaceDemo::initPhysics()
|
||||
{
|
||||
|
||||
//m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
|
||||
m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
|
||||
|
||||
btMatrix3x3 basisA;
|
||||
basisA.setIdentity();
|
||||
@@ -69,7 +70,10 @@ void CollisionInterfaceDemo::initPhysics()
|
||||
objects[1].getWorldTransform().setBasis(basisB);
|
||||
|
||||
btBoxShape* boxA = new btBoxShape(btVector3(1,1,1));
|
||||
boxA->setMargin(0.f);
|
||||
|
||||
btBoxShape* boxB = new btBoxShape(btVector3(0.5,0.5,0.5));
|
||||
boxB->setMargin(0.f);
|
||||
//ConvexHullShape hullA(points0,3);
|
||||
//hullA.setLocalScaling(btVector3(3,3,3));
|
||||
//ConvexHullShape hullB(points1,4);
|
||||
@@ -89,9 +93,13 @@ void CollisionInterfaceDemo::initPhysics()
|
||||
//SimpleBroadphase* broadphase = new btSimpleBroadphase;
|
||||
|
||||
collisionWorld = new btCollisionWorld(dispatcher,broadphase,collisionConfiguration);
|
||||
collisionWorld->setDebugDrawer(&debugDrawer);
|
||||
|
||||
#ifdef TEST_NOT_ADDING_OBJECTS_TO_WORLD
|
||||
collisionWorld->addCollisionObject(&objects[0]);
|
||||
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);
|
||||
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;
|
||||
|
||||
if (collisionWorld)
|
||||
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();
|
||||
for (i=0;i<numManifolds;i++)
|
||||
@@ -137,7 +164,7 @@ void CollisionInterfaceDemo::displayCallback(void) {
|
||||
btManifoldPoint& pt = contactManifold->getContactPoint(j);
|
||||
|
||||
glBegin(GL_LINES);
|
||||
glColor3f(1, 0, 1);
|
||||
glColor3f(0, 0, 0);
|
||||
|
||||
btVector3 ptA = pt.getPositionWorldOnA();
|
||||
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
|
||||
//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();
|
||||
|
||||
btScalar m[16];
|
||||
|
||||
btVector3 worldBoundsMin,worldBoundsMax;
|
||||
collisionWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
|
||||
|
||||
btQuaternion qA = objects[0].getWorldTransform().getRotation();
|
||||
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();
|
||||
glutSwapBuffers();
|
||||
}
|
||||
@@ -184,7 +266,14 @@ void CollisionInterfaceDemo::displayCallback(void) {
|
||||
void CollisionInterfaceDemo::clientResetScene()
|
||||
{
|
||||
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));
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -601,17 +601,25 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
|
||||
7,2,3,
|
||||
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);
|
||||
int si=36;
|
||||
for (int i=0;i<si;i+=3)
|
||||
{
|
||||
btVector3 v1 = vertices[indices[i]]*halfExtent;
|
||||
btVector3 v2 = vertices[indices[i+1]]*halfExtent;
|
||||
btVector3 v3 = vertices[indices[i+2]]*halfExtent;
|
||||
const btVector3& v1 = vertices[indices[i]];;
|
||||
const btVector3& v2 = vertices[indices[i+1]];
|
||||
const btVector3& v3 = vertices[indices[i+2]];
|
||||
btVector3 normal = (v3-v1).cross(v2-v1);
|
||||
normal.normalize ();
|
||||
|
||||
glNormal3f(normal.getX(),normal.getY(),normal.getZ());
|
||||
glVertex3f (v1.x(), v1.y(), v1.z());
|
||||
glVertex3f (v2.x(), v2.y(), v2.z());
|
||||
@@ -619,6 +627,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
|
||||
|
||||
}
|
||||
glEnd();
|
||||
#endif
|
||||
|
||||
useWireframeFallback = false;
|
||||
break;
|
||||
@@ -812,8 +821,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
|
||||
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);
|
||||
int i;
|
||||
@@ -821,7 +828,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
|
||||
{
|
||||
btVector3 vtx;
|
||||
polyshape->getVertex(i,vtx);
|
||||
glRasterPos3f(vtx.x(), vtx.y(), vtx.z());
|
||||
char buf[12];
|
||||
sprintf(buf," %d",i);
|
||||
//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);
|
||||
btScalar d = vtx.dot(normal);
|
||||
|
||||
glRasterPos3f(normal.x()*d, normal.y()*d, normal.z()*d);
|
||||
char buf[12];
|
||||
sprintf(buf," plane %d",i);
|
||||
//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();
|
||||
|
||||
|
||||
@@ -1,4 +1,3 @@
|
||||
|
||||
/*
|
||||
* 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.
|
||||
@@ -424,6 +423,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
|
||||
output.addContactPoint(-normal,pointInWorld,-*depth);
|
||||
#else
|
||||
output.addContactPoint(-normal,pb,-*depth);
|
||||
|
||||
#endif //
|
||||
*return_code = code;
|
||||
}
|
||||
@@ -593,21 +593,30 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
|
||||
if (maxc < 1) maxc = 1;
|
||||
|
||||
if (cnum <= maxc) {
|
||||
|
||||
if (code<4)
|
||||
{
|
||||
// we have less contacts than we need, so we use them all
|
||||
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];
|
||||
|
||||
for (j=0; j < cnum; j++)
|
||||
{
|
||||
btVector3 pointInWorld;
|
||||
for (i=0; i<3; i++)
|
||||
pointInWorld[i] = point[j*3+i] + pa[i];
|
||||
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 {
|
||||
// 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;
|
||||
for (i=0; i<3; 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;
|
||||
}
|
||||
|
||||
@@ -42,6 +42,24 @@ subject to the following restrictions:
|
||||
#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)
|
||||
:m_dispatcher1(dispatcher),
|
||||
m_broadphasePairCache(pairCache),
|
||||
@@ -92,27 +110,27 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
|
||||
btAssert(collisionObject);
|
||||
|
||||
//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
|
||||
btTransform trans = collisionObject->getWorldTransform();
|
||||
//calculate new AABB
|
||||
btTransform trans = collisionObject->getWorldTransform();
|
||||
|
||||
btVector3 minAabb;
|
||||
btVector3 maxAabb;
|
||||
collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
|
||||
btVector3 minAabb;
|
||||
btVector3 maxAabb;
|
||||
collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
|
||||
|
||||
int type = collisionObject->getCollisionShape()->getShapeType();
|
||||
collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
|
||||
minAabb,
|
||||
maxAabb,
|
||||
type,
|
||||
collisionObject,
|
||||
collisionFilterGroup,
|
||||
collisionFilterMask,
|
||||
m_dispatcher1,0
|
||||
)) ;
|
||||
int type = collisionObject->getCollisionShape()->getShapeType();
|
||||
collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
|
||||
minAabb,
|
||||
maxAabb,
|
||||
type,
|
||||
collisionObject,
|
||||
collisionFilterGroup,
|
||||
collisionFilterMask,
|
||||
m_dispatcher1,0
|
||||
)) ;
|
||||
|
||||
|
||||
|
||||
@@ -228,10 +246,10 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
|
||||
|
||||
|
||||
void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
|
||||
btCollisionObject* collisionObject,
|
||||
const btCollisionShape* collisionShape,
|
||||
const btTransform& colObjWorldTransform,
|
||||
RayResultCallback& resultCallback)
|
||||
btCollisionObject* collisionObject,
|
||||
const btCollisionShape* collisionShape,
|
||||
const btTransform& colObjWorldTransform,
|
||||
RayResultCallback& resultCallback)
|
||||
{
|
||||
btSphereShape pointShape(btScalar(0.0));
|
||||
pointShape.setMargin(0.f);
|
||||
@@ -239,7 +257,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
|
||||
|
||||
if (collisionShape->isConvex())
|
||||
{
|
||||
// BT_PROFILE("rayTestConvex");
|
||||
// BT_PROFILE("rayTestConvex");
|
||||
btConvexCast::CastResult castResult;
|
||||
castResult.m_fraction = resultCallback.m_closestHitFraction;
|
||||
|
||||
@@ -268,10 +286,10 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
|
||||
castResult.m_normal.normalize();
|
||||
btCollisionWorld::LocalRayResult localRayResult
|
||||
(
|
||||
collisionObject,
|
||||
0,
|
||||
castResult.m_normal,
|
||||
castResult.m_fraction
|
||||
collisionObject,
|
||||
0,
|
||||
castResult.m_normal,
|
||||
castResult.m_fraction
|
||||
);
|
||||
|
||||
bool normalInWorldSpace = true;
|
||||
@@ -283,7 +301,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
|
||||
} else {
|
||||
if (collisionShape->isConcave())
|
||||
{
|
||||
// BT_PROFILE("rayTestConcave");
|
||||
// BT_PROFILE("rayTestConcave");
|
||||
if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
|
||||
{
|
||||
///optimized version for btBvhTriangleMeshShape
|
||||
@@ -299,18 +317,18 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
|
||||
btCollisionObject* m_collisionObject;
|
||||
btTriangleMeshShape* m_triangleMesh;
|
||||
|
||||
btTransform m_colObjWorldTransform;
|
||||
btTransform m_colObjWorldTransform;
|
||||
|
||||
BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
|
||||
btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform):
|
||||
//@BP Mod
|
||||
btTriangleRaycastCallback(from,to, resultCallback->m_flags),
|
||||
m_resultCallback(resultCallback),
|
||||
m_collisionObject(collisionObject),
|
||||
m_triangleMesh(triangleMesh),
|
||||
m_colObjWorldTransform(colObjWorldTransform)
|
||||
{
|
||||
}
|
||||
//@BP Mod
|
||||
btTriangleRaycastCallback(from,to, resultCallback->m_flags),
|
||||
m_resultCallback(resultCallback),
|
||||
m_collisionObject(collisionObject),
|
||||
m_triangleMesh(triangleMesh),
|
||||
m_colObjWorldTransform(colObjWorldTransform)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
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_triangleIndex = triangleIndex;
|
||||
|
||||
btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
|
||||
btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
|
||||
|
||||
btCollisionWorld::LocalRayResult rayResult
|
||||
(m_collisionObject,
|
||||
(m_collisionObject,
|
||||
&shapeInfo,
|
||||
hitNormalWorld,
|
||||
hitFraction);
|
||||
@@ -354,18 +372,18 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
|
||||
btCollisionObject* m_collisionObject;
|
||||
btConcaveShape* m_triangleMesh;
|
||||
|
||||
btTransform m_colObjWorldTransform;
|
||||
btTransform m_colObjWorldTransform;
|
||||
|
||||
BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
|
||||
btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
|
||||
//@BP Mod
|
||||
btTriangleRaycastCallback(from,to, resultCallback->m_flags),
|
||||
m_resultCallback(resultCallback),
|
||||
m_collisionObject(collisionObject),
|
||||
m_triangleMesh(triangleMesh),
|
||||
m_colObjWorldTransform(colObjWorldTransform)
|
||||
{
|
||||
}
|
||||
//@BP Mod
|
||||
btTriangleRaycastCallback(from,to, resultCallback->m_flags),
|
||||
m_resultCallback(resultCallback),
|
||||
m_collisionObject(collisionObject),
|
||||
m_triangleMesh(triangleMesh),
|
||||
m_colObjWorldTransform(colObjWorldTransform)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
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_triangleIndex = triangleIndex;
|
||||
|
||||
btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
|
||||
btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
|
||||
|
||||
btCollisionWorld::LocalRayResult rayResult
|
||||
(m_collisionObject,
|
||||
(m_collisionObject,
|
||||
&shapeInfo,
|
||||
hitNormalWorld,
|
||||
hitFraction);
|
||||
@@ -400,7 +418,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
|
||||
concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
|
||||
}
|
||||
} else {
|
||||
// BT_PROFILE("rayTestCompound");
|
||||
// BT_PROFILE("rayTestCompound");
|
||||
///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
|
||||
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,
|
||||
btCollisionObject* collisionObject,
|
||||
const btCollisionShape* collisionShape,
|
||||
const btTransform& colObjWorldTransform,
|
||||
ConvexResultCallback& resultCallback, btScalar allowedPenetration)
|
||||
btCollisionObject* collisionObject,
|
||||
const btCollisionShape* collisionShape,
|
||||
const btTransform& colObjWorldTransform,
|
||||
ConvexResultCallback& resultCallback, btScalar allowedPenetration)
|
||||
{
|
||||
if (collisionShape->isConvex())
|
||||
{
|
||||
@@ -443,15 +461,15 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
|
||||
btConvexShape* convexShape = (btConvexShape*) collisionShape;
|
||||
btVoronoiSimplexSolver simplexSolver;
|
||||
btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
|
||||
|
||||
|
||||
btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
|
||||
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
|
||||
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
|
||||
|
||||
btConvexCast* castPtr = &convexCaster1;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
|
||||
{
|
||||
//add hit
|
||||
@@ -461,13 +479,13 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
|
||||
{
|
||||
castResult.m_normal.normalize();
|
||||
btCollisionWorld::LocalConvexResult localConvexResult
|
||||
(
|
||||
collisionObject,
|
||||
0,
|
||||
castResult.m_normal,
|
||||
castResult.m_hitPoint,
|
||||
castResult.m_fraction
|
||||
);
|
||||
(
|
||||
collisionObject,
|
||||
0,
|
||||
castResult.m_normal,
|
||||
castResult.m_hitPoint,
|
||||
castResult.m_fraction
|
||||
);
|
||||
|
||||
bool normalInWorldSpace = true;
|
||||
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,
|
||||
btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
|
||||
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
|
||||
m_resultCallback(resultCallback),
|
||||
m_collisionObject(collisionObject),
|
||||
m_triangleMesh(triangleMesh)
|
||||
{
|
||||
}
|
||||
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
|
||||
m_resultCallback(resultCallback),
|
||||
m_collisionObject(collisionObject),
|
||||
m_triangleMesh(triangleMesh)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
(m_collisionObject,
|
||||
(m_collisionObject,
|
||||
&shapeInfo,
|
||||
hitNormalLocal,
|
||||
hitPointLocal,
|
||||
@@ -554,12 +572,12 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
|
||||
|
||||
BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
|
||||
btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
|
||||
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
|
||||
m_resultCallback(resultCallback),
|
||||
m_collisionObject(collisionObject),
|
||||
m_triangleMesh(triangleMesh)
|
||||
{
|
||||
}
|
||||
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
|
||||
m_resultCallback(resultCallback),
|
||||
m_collisionObject(collisionObject),
|
||||
m_triangleMesh(triangleMesh)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
(m_collisionObject,
|
||||
(m_collisionObject,
|
||||
&shapeInfo,
|
||||
hitNormalLocal,
|
||||
hitPointLocal,
|
||||
@@ -641,10 +659,10 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
|
||||
btCollisionWorld::RayResultCallback& m_resultCallback;
|
||||
|
||||
btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
|
||||
:m_rayFromWorld(rayFromWorld),
|
||||
m_rayToWorld(rayToWorld),
|
||||
m_world(world),
|
||||
m_resultCallback(resultCallback)
|
||||
:m_rayFromWorld(rayFromWorld),
|
||||
m_rayToWorld(rayToWorld),
|
||||
m_world(world),
|
||||
m_resultCallback(resultCallback)
|
||||
{
|
||||
m_rayFromTrans.setIdentity();
|
||||
m_rayFromTrans.setOrigin(m_rayFromWorld);
|
||||
@@ -666,7 +684,7 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
virtual bool process(const btBroadphaseProxy* proxy)
|
||||
{
|
||||
@@ -697,9 +715,9 @@ struct btSingleRayCallback : public btBroadphaseRayCallback
|
||||
{
|
||||
m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
|
||||
collisionObject,
|
||||
collisionObject->getCollisionShape(),
|
||||
collisionObject->getWorldTransform(),
|
||||
m_resultCallback);
|
||||
collisionObject->getCollisionShape(),
|
||||
collisionObject->getWorldTransform(),
|
||||
m_resultCallback);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
@@ -771,13 +789,13 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback
|
||||
if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
|
||||
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
|
||||
m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
|
||||
collisionObject,
|
||||
collisionObject->getCollisionShape(),
|
||||
collisionObject->getWorldTransform(),
|
||||
m_resultCallback,
|
||||
m_allowedCcdPenetration);
|
||||
collisionObject,
|
||||
collisionObject->getCollisionShape(),
|
||||
collisionObject->getWorldTransform(),
|
||||
m_resultCallback,
|
||||
m_allowedCcdPenetration);
|
||||
}
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
};
|
||||
@@ -792,7 +810,7 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT
|
||||
/// and for each object with ray-aabb overlap, perform an exact ray test
|
||||
/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
|
||||
|
||||
|
||||
|
||||
|
||||
btTransform convexFromTrans,convexToTrans;
|
||||
convexFromTrans = convexFromWorld;
|
||||
@@ -835,12 +853,355 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT
|
||||
{
|
||||
objectQuerySingle(castShape, convexFromTrans,convexToTrans,
|
||||
collisionObject,
|
||||
collisionObject->getCollisionShape(),
|
||||
collisionObject->getWorldTransform(),
|
||||
resultCallback,
|
||||
allowedCcdPenetration);
|
||||
collisionObject->getCollisionShape(),
|
||||
collisionObject->getWorldTransform(),
|
||||
resultCallback,
|
||||
allowedCcdPenetration);
|
||||
}
|
||||
}
|
||||
}
|
||||
#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);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -150,6 +150,12 @@ public:
|
||||
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
|
||||
///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -77,8 +77,7 @@ protected:
|
||||
|
||||
virtual void saveKinematicState(btScalar timeStep);
|
||||
|
||||
void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
@@ -135,7 +134,6 @@ public:
|
||||
///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
|
||||
virtual void removeCollisionObject(btCollisionObject* collisionObject);
|
||||
|
||||
void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
|
||||
|
||||
void debugDrawConstraint(btTypedConstraint* constraint);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user