From 6f80b98a67ffce58b8ef55b17fe16f8914399a82 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Thu, 6 Dec 2007 02:54:29 +0000 Subject: [PATCH] - fix issue with convex cast: results further away (larger hitfraction) could overwrite closer results - minor naming convention thing (variables start with lower case) - renamed MotorDemo - added ConcaveConvexcastDemo, Thanks John McCutchan (JMC) --- Demos/AllBulletDemos/DemoEntries.cpp | 4 + .../ConcaveConvexcastDemo.cpp | 589 ++++++++++++++++++ .../ConcaveConvexcastDemo.h | 84 +++ Demos/ConcaveConvexcastDemo/main.cpp | 15 + .../MotorDemo.cpp | 0 .../MotorDemo.h | 12 - .../main.cpp | 0 .../CollisionDispatch/btCollisionWorld.cpp | 229 ++++--- .../CollisionDispatch/btCollisionWorld.h | 4 +- .../CollisionShapes/btOptimizedBvh.cpp | 19 +- 10 files changed, 855 insertions(+), 101 deletions(-) create mode 100644 Demos/ConcaveConvexcastDemo/ConcaveConvexcastDemo.cpp create mode 100644 Demos/ConcaveConvexcastDemo/ConcaveConvexcastDemo.h create mode 100644 Demos/ConcaveConvexcastDemo/main.cpp rename Demos/{DynamicControl => DynamicControlDemo}/MotorDemo.cpp (100%) rename Demos/{DynamicControl => DynamicControlDemo}/MotorDemo.h (70%) rename Demos/{DynamicControl => DynamicControlDemo}/main.cpp (100%) diff --git a/Demos/AllBulletDemos/DemoEntries.cpp b/Demos/AllBulletDemos/DemoEntries.cpp index 8b9c17fab..6beba04eb 100644 --- a/Demos/AllBulletDemos/DemoEntries.cpp +++ b/Demos/AllBulletDemos/DemoEntries.cpp @@ -20,7 +20,9 @@ subject to the following restrictions: #include "../BasicDemo/BasicDemo.h" #include "../ConcaveDemo/ConcaveDemo.h" #include "../ConcaveRaycastDemo/ConcaveRaycastDemo.h" +#include "../ConcaveConvexcastDemo/ConcaveConvexcastDemo.h" #include "../ConvexDecompositionDemo/ConvexDecompositionDemo.h" +#include "../DynamicControlDemo/MotorDemo.h" #include "../RagdollDemo/RagdollDemo.h" #include "../GimpactTestDemo/GimpactTestDemo.h" #include "../Raytracer/Raytracer.h" @@ -94,10 +96,12 @@ public: btDemoEntry g_demoEntries[] = { + {"DynamicControlDemo",MotorDemo::Create}, {"RagdollDemo",RagdollDemo::Create}, {"CcdPhysicsDemo", CcdPhysicsDemo::Create}, {"ConcaveDemo",ConcaveDemo::Create}, {"ConcaveRaycastDemo",ConcaveRaycastDemo::Create}, + {"ConcaveConvexcastDemo",ConcaveConvexcastDemo::Create}, {"ConvexDecomposition",ConvexDecompositionDemo::Create}, {"BasicDemo", BasicDemo::Create}, {"BspDemo", BspDemo::Create}, diff --git a/Demos/ConcaveConvexcastDemo/ConcaveConvexcastDemo.cpp b/Demos/ConcaveConvexcastDemo/ConcaveConvexcastDemo.cpp new file mode 100644 index 000000000..eca4477f4 --- /dev/null +++ b/Demos/ConcaveConvexcastDemo/ConcaveConvexcastDemo.cpp @@ -0,0 +1,589 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btBulletDynamicsCommon.h" +#include "LinearMath/btIDebugDraw.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "GLDebugDrawer.h" +#include "ConcaveConvexcastDemo.h" +#include "GL_ShapeDrawer.h" +#include "GlutStuff.h" + +//#define USE_PARALLEL_DISPATCHER 1 +#ifdef USE_PARALLEL_DISPATCHER +#include "../../Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h" +#include "../../Extras/BulletMultiThreaded/Win32ThreadSupport.h" +#include "../../Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h" +#endif//USE_PARALLEL_DISPATCHER + + + + +static btVector3* gVertices=0; +static int* gIndices=0; +static btBvhTriangleMeshShape* trimeshShape =0; +static btRigidBody* staticBody = 0; +static float waveheight = 5.f; + +const float TRIANGLE_SIZE=8.f; + + +/* Scrolls back and forth over terrain */ +#define NUMRAYS_IN_BAR 100 +class btConvexcastBatch +{ +public: + btVector3 source[NUMRAYS_IN_BAR]; + btVector3 dest[NUMRAYS_IN_BAR]; + btVector3 direction[NUMRAYS_IN_BAR]; + btVector3 hit[NUMRAYS_IN_BAR]; + btVector3 normal[NUMRAYS_IN_BAR]; + + int frame_counter; + int ms; + int sum_ms; + int sum_ms_samples; + int min_ms; + int max_ms; + + btClock frame_timer; + + btScalar dx; + btScalar min_x; + btScalar max_x; + btScalar min_y; + btScalar max_y; + btScalar sign; + + btVector3 boxShapeHalfExtents; + btBoxShape boxShape; + + btConvexcastBatch () : boxShape(btVector3(0.0, 0.0, 0.0)) + { + ms = 0; + max_ms = 0; + min_ms = 9999.0; + sum_ms_samples = 0; + sum_ms = 0; + } + + btConvexcastBatch (bool unused, btScalar ray_length, btScalar min_z, btScalar max_z, btScalar min_y = -10, btScalar max_y = 10) : boxShape(btVector3(0.0, 0.0, 0.0)) + { + boxShapeHalfExtents = btVector3(1.0, 1.0, 1.0); + boxShape = btBoxShape(boxShapeHalfExtents); + frame_counter = 0; + ms = 0; + max_ms = 0; + min_ms = 9999.0; + sum_ms_samples = 0; + sum_ms = 0; + dx = 10.0; + min_x = -40; + max_x = 20; + this->min_y = min_y; + this->max_y = max_y; + sign = 1.0; + btScalar dalpha = 2*SIMD_2_PI/NUMRAYS_IN_BAR; + for (int i = 0; i < NUMRAYS_IN_BAR; i++) + { + btScalar z = (max_z-min_z)/NUMRAYS_IN_BAR * i + min_z; + source[i] = btVector3(min_x, max_y, z); + dest[i] = btVector3(min_x + ray_length, min_y, z); + normal[i] = btVector3(1.0, 0.0, 0.0); + } + } + + btConvexcastBatch (btScalar ray_length, btScalar z, btScalar min_y = -1000, btScalar max_y = 10) : boxShape(btVector3(0.0, 0.0, 0.0)) + { + boxShapeHalfExtents = btVector3(1.0, 1.0, 1.0); + boxShape = btBoxShape(boxShapeHalfExtents); + frame_counter = 0; + ms = 0; + max_ms = 0; + min_ms = 9999.0; + sum_ms_samples = 0; + sum_ms = 0; + dx = 10.0; + min_x = -40; + max_x = 20; + this->min_y = min_y; + this->max_y = max_y; + sign = 1.0; + btScalar dalpha = 2*SIMD_2_PI/NUMRAYS_IN_BAR; + for (int i = 0; i < NUMRAYS_IN_BAR; i++) + { + btScalar alpha = dalpha * i; + // rotate around by alpha degrees y + btQuaternion q(btVector3(0.0, 1.0, 0.0), alpha); + direction[i] = btVector3(1.0, 0.0, 0.0); + direction[i] = q * direction[i]; + direction[i] = direction[i]; + source[i] = btVector3(min_x, max_y, z); + dest[i] = source[i] + direction[i] * ray_length; + dest[i][1] = min_y; + normal[i] = btVector3(1.0, 0.0, 0.0); + } + } + + void move (btScalar dt) + { + if (dt > (1.0/60.0)) + dt = 1.0/60.0; + for (int i = 0; i < NUMRAYS_IN_BAR; i++) + { + source[i][0] += dx * dt * sign; + dest[i][0] += dx * dt * sign; + } + if (source[0][0] < min_x) + sign = 1.0; + else if (source[0][0] > max_x) + sign = -1.0; + } + + void cast (btCollisionWorld* cw) + { + frame_timer.reset (); + for (int i = 0; i < NUMRAYS_IN_BAR; i++) + { + btCollisionWorld::ClosestConvexResultCallback cb(source[i], dest[i]); + + cw->convexTest (&boxShape, source[i], dest[i], cb); + if (cb.HasHit ()) + { + hit[i] = cb.m_hitPointWorld; + normal[i] = cb.m_hitNormalWorld; + normal[i].normalize (); + } else { + hit[i] = dest[i]; + normal[i] = btVector3(1.0, 0.0, 0.0); + } + + } + ms += frame_timer.getTimeMilliseconds (); + frame_counter++; + if (frame_counter > 50) + { + min_ms = ms < min_ms ? ms : min_ms; + max_ms = ms > max_ms ? ms : max_ms; + sum_ms += ms; + sum_ms_samples++; + btScalar mean_ms = (btScalar)sum_ms/(btScalar)sum_ms_samples; + printf("%d rays in %d ms %d %d %f\n", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms); + ms = 0; + frame_counter = 0; + } + } + + + void drawCube (const btVector3& com) + { + glPushMatrix (); + glTranslatef (com[0], com[1], com[2]); + glScalef (2.0 * boxShapeHalfExtents[0], 2.0 * boxShapeHalfExtents[1], 2.0 * boxShapeHalfExtents[2]); + glutSolidCube (1.0); + glPopMatrix (); + } + + void draw () + { + glDisable (GL_LIGHTING); + glColor3f (0.0, 1.0, 0.0); + glBegin (GL_LINES); + for (int i = 0; i < NUMRAYS_IN_BAR; i++) + { + glVertex3f (source[i][0], source[i][1], source[i][2]); + glVertex3f (hit[i][0], hit[i][1], hit[i][2]); + } + glColor3f (1.0, 1.0, 1.0); + glBegin (GL_LINES); + for (int i = 0; i < NUMRAYS_IN_BAR; i++) + { + glVertex3f (hit[i][0], hit[i][1], hit[i][2]); + glVertex3f (hit[i][0] + normal[i][0], hit[i][1] + normal[i][1], hit[i][2] + normal[i][2]); + } + glEnd (); + glColor3f (0.0, 1.0, 1.0); + for (int i = 0; i < NUMRAYS_IN_BAR; i++) + { + drawCube (hit[i]); + } + glEnable (GL_LIGHTING); + } +}; + + +static btConvexcastBatch convexcastBatch; + +///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; +inline btScalar calculateCombinedFriction(float friction0,float friction1) +{ + btScalar friction = friction0 * friction1; + + const btScalar MAX_FRICTION = 10.f; + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; + +} + +inline btScalar calculateCombinedRestitution(float restitution0,float restitution1) +{ + return restitution0 * restitution1; +} + + + +static bool CustomMaterialCombinerCallback(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) +{ + + float friction0 = colObj0->getFriction(); + float friction1 = colObj1->getFriction(); + float restitution0 = colObj0->getRestitution(); + float restitution1 = colObj1->getRestitution(); + + if (colObj0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) + { + friction0 = 1.0;//partId0,index0 + restitution0 = 0.f; + } + if (colObj1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) + { + if (index1&1) + { + friction1 = 1.0f;//partId1,index1 + } else + { + friction1 = 0.f; + } + restitution1 = 0.f; + } + + cp.m_combinedFriction = calculateCombinedFriction(friction0,friction1); + cp.m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1); + + //this return value is currently ignored, but to be on the safe side: return false if you don't calculate friction + return true; +} + +extern ContactAddedCallback gContactAddedCallback; + + const int NUM_VERTS_X = 30; + const int NUM_VERTS_Y = 30; + const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y; + +void ConcaveConvexcastDemo::setVertexPositions(float waveheight, float offset) +{ + int i; + int j; + + for ( i=0;isetCollisionFlags( staticBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); + staticBody->setActivationState(DISABLE_DEACTIVATION); + } else + { + staticBody->setCollisionFlags( staticBody->getCollisionFlags() & ~btCollisionObject::CF_KINEMATIC_OBJECT); + staticBody->forceActivationState(ACTIVE_TAG); + } + } + + DemoApplication::keyboardCallback(key,x,y); + +} + +void ConcaveConvexcastDemo::initPhysics() +{ + #define TRISIZE 10.f + + gContactAddedCallback = CustomMaterialCombinerCallback; + +#define USE_TRIMESH_SHAPE 1 +#ifdef USE_TRIMESH_SHAPE + + int vertStride = sizeof(btVector3); + int indexStride = 3*sizeof(int); + + + const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1); + + gVertices = new btVector3[totalVerts]; + gIndices = new int[totalTriangles*3]; + + int i; + + + setVertexPositions(waveheight,0.f); + + int index=0; + for ( i=0;igetOptimizedBvh()->calculateSerializeBufferSize(); + buffer = btAlignedAlloc(numBytes,16); + bool swapEndian = false; + trimeshShape->getOptimizedBvh()->serialize(buffer,numBytes,swapEndian); + FILE* file = fopen("bvh.bin","wb"); + fwrite(buffer,1,numBytes,file); + fclose(file); + btAlignedFree(buffer); + + + +#else + + trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,false); + + char* fileName = "bvh.bin"; + + FILE* file = fopen(fileName,"rb"); + int size=0; + btOptimizedBvh* bvh = 0; + + if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET)) { /* File operations denied? ok, just close and return failure */ + printf("Error: cannot get filesize from %s\n", fileName); + exit(0); + } else + { + + fseek(file, 0, SEEK_SET); + + int buffersize = size+btOptimizedBvh::getAlignmentSerializationPadding(); + + void* buffer = btAlignedAlloc(buffersize,16); + int read = fread(buffer,1,size,file); + fclose(file); + bool swapEndian = false; + bvh = btOptimizedBvh::deSerializeInPlace(buffer,buffersize,swapEndian); + } + + trimeshShape->setOptimizedBvh(bvh); + +#endif + + btCollisionShape* groundShape = trimeshShape; + +#else + btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50)); + m_collisionShapes.push_back(groundShape); + +#endif //USE_TRIMESH_SHAPE + + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + +#ifdef USE_PARALLEL_DISPATCHER + +#ifdef USE_WIN32_THREADING + + int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks + Win32ThreadSupport* threadSupport = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo( + "collision", + processCollisionTask, + createCollisionLocalStoreMemory, + maxNumOutstandingTasks)); +#else +///todo other platform threading +///Playstation 3 SPU (SPURS) version is available through PS3 Devnet +///Libspe2 SPU support will be available soon +///pthreads version +///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface +#endif + + m_dispatcher = new SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks,m_collisionConfiguration); +#else + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); +#endif//USE_PARALLEL_DISPATCHER + + + btVector3 worldMin(-1000,-1000,-1000); + btVector3 worldMax(1000,1000,1000); + m_broadphase = new btAxisSweep3(worldMin,worldMax); + m_solver = new btSequentialImpulseConstraintSolver(); + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); +#ifdef USE_PARALLEL_DISPATCHER + m_dynamicsWorld->getDispatchInfo().m_enableSPU=true; +#endif //USE_PARALLEL_DISPATCHER + + float mass = 0.f; + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0,-2,0)); + + btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1)); + m_collisionShapes.push_back(colShape); + + { + for (int i=0;i<10;i++) + { + //btCollisionShape* colShape = new btCapsuleShape(0.5,2.0);//boxShape = new btSphereShape(1.f); + startTransform.setOrigin(btVector3(2*i,10,1)); + localCreateRigidBody(1, startTransform,colShape); + } + } + + startTransform.setIdentity(); + staticBody = localCreateRigidBody(mass, startTransform,groundShape); + + staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT); + + //enable custom material callback + staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + + convexcastBatch = btConvexcastBatch (40.0, 0.0, -10.0); + //convexcastBatch = btConvexcastBatch (true, 40.0, -50.0, 50.0); +} + +void ConcaveConvexcastDemo::clientMoveAndDisplay() +{ + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + float dt = m_clock.getTimeMicroseconds() * 0.000001f; + m_clock.reset(); + + if (m_animatedMesh) + { + static float offset=0.f; + offset+=0.01f; + + setVertexPositions(waveheight,offset); + + trimeshShape->refitTree(); + + //clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation. + m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher()); + } + + m_dynamicsWorld->stepSimulation(dt); + + convexcastBatch.move (dt); + convexcastBatch.cast (m_dynamicsWorld); + renderme(); + convexcastBatch.draw (); + glFlush(); + glutSwapBuffers(); + +} + + + + +void ConcaveConvexcastDemo::displayCallback(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + renderme(); + convexcastBatch.draw (); + glFlush(); + glutSwapBuffers(); +} + + + +void ConcaveConvexcastDemo::exitPhysics() +{ + + + + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject( obj ); + delete obj; + } + + //delete collision shapes + for (int j=0;j m_collisionShapes; + + btTriangleIndexVertexArray* m_indexVertexArrays; + + btBroadphaseInterface* m_broadphase; + + btCollisionDispatcher* m_dispatcher; + + btConstraintSolver* m_solver; + + btDefaultCollisionConfiguration* m_collisionConfiguration; + + bool m_animatedMesh; + + public: + + ConcaveConvexcastDemo() : m_animatedMesh(false) + { + + } + void initPhysics(); + + void exitPhysics(); + + virtual ~ConcaveConvexcastDemo() + { + exitPhysics(); + } + + virtual void clientMoveAndDisplay(); + + virtual void displayCallback(); + + //to show refit works + void setVertexPositions(float waveheight, float offset); + + virtual void keyboardCallback(unsigned char key, int x, int y); + + static DemoApplication* Create() + { + ConcaveConvexcastDemo* demo = new ConcaveConvexcastDemo(); + demo->myinit(); + demo->initPhysics(); + return demo; + }; +}; + +#endif //CONCAVE_CONVEXCAST_DEMO_H + diff --git a/Demos/ConcaveConvexcastDemo/main.cpp b/Demos/ConcaveConvexcastDemo/main.cpp new file mode 100644 index 000000000..6f405b6a7 --- /dev/null +++ b/Demos/ConcaveConvexcastDemo/main.cpp @@ -0,0 +1,15 @@ + +#include "ConcaveConvexcastDemo.h" +#include "GlutStuff.h" + + +int main(int argc,char** argv) +{ + + ConcaveConvexcastDemo* concaveConvexcastDemo = new ConcaveConvexcastDemo(); + concaveConvexcastDemo->initPhysics(); + concaveConvexcastDemo->setCameraDistance(30.f); + + return glutmain(argc, argv,640,480,"Concave Convexcast Demo",concaveConvexcastDemo); +} + diff --git a/Demos/DynamicControl/MotorDemo.cpp b/Demos/DynamicControlDemo/MotorDemo.cpp similarity index 100% rename from Demos/DynamicControl/MotorDemo.cpp rename to Demos/DynamicControlDemo/MotorDemo.cpp diff --git a/Demos/DynamicControl/MotorDemo.h b/Demos/DynamicControlDemo/MotorDemo.h similarity index 70% rename from Demos/DynamicControl/MotorDemo.h rename to Demos/DynamicControlDemo/MotorDemo.h index 3e33b0518..732a95b86 100644 --- a/Demos/DynamicControl/MotorDemo.h +++ b/Demos/DynamicControlDemo/MotorDemo.h @@ -13,18 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -*/ - #ifndef MOTORDEMO_H #define MOTORDEMO_H diff --git a/Demos/DynamicControl/main.cpp b/Demos/DynamicControlDemo/main.cpp similarity index 100% rename from Demos/DynamicControl/main.cpp rename to Demos/DynamicControlDemo/main.cpp diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 21c7ee6ab..f4fd7abfe 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -238,49 +238,110 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra } else { if (collisionShape->isConcave()) { - btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); - btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); - - //ConvexCast::CastResult - struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { - btCollisionWorld::RayResultCallback* m_resultCallback; - btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; + ///optimized version for btBvhTriangleMeshShape + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); - BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): - btTriangleRaycastCallback(from,to), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) + //ConvexCast::CastResult + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): + btTriangleRaycastCallback(from,to), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + btCollisionWorld::LocalRayResult rayResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitFraction); + + bool normalInWorldSpace = false; + return m_resultCallback->AddSingleResult(rayResult,normalInWorldSpace); } + }; - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); + } else + { + btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape; + + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + //ConvexCast::CastResult + + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - - btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitFraction); - - bool normalInWorldSpace = false; - return m_resultCallback->AddSingleResult(rayResult,normalInWorldSpace); - } + btCollisionWorld::RayResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; - }; + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): + btTriangleRaycastCallback(from,to), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); - rcb.m_hitFraction = resultCallback.m_closestHitFraction; - triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); + + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + btCollisionWorld::LocalRayResult rayResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitFraction); + + bool normalInWorldSpace = false; + return m_resultCallback->AddSingleResult(rayResult,normalInWorldSpace); + + + } + + }; + + + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + + btVector3 rayAabbMinLocal = rayFromLocal; + rayAabbMinLocal.setMin(rayToLocal); + btVector3 rayAabbMaxLocal = rayFromLocal; + rayAabbMaxLocal.setMax(rayToLocal); + + triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); + } } else { //todo: use AABB tree or other BVH acceleration structure! if (collisionShape->isCompound()) @@ -356,55 +417,69 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt } else { if (collisionShape->isConcave()) { - btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); - btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); - btTransform rotationXform; - - rotationXform.setIdentity (); // FIXME!!! - - //ConvexCast::CastResult - struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback + if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { - btCollisionWorld::ConvexResultCallback* m_resultCallback; - btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); + btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); + btTransform rotationXform; - 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), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) + rotationXform.setIdentity (); // FIXME!!! + + //ConvexCast::CastResult + struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback + { + btCollisionWorld::ConvexResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + 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), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + if (hitFraction <= m_resultCallback->m_closestHitFraction) + { + + btCollisionWorld::LocalConvexResult convexResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); + + bool normalInWorldSpace = false; + + + return m_resultCallback->AddSingleResult(convexResult,normalInWorldSpace); + } + return hitFraction; } + }; - virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); - - bool normalInWorldSpace = false; - return m_resultCallback->AddSingleResult(convexResult,normalInWorldSpace); - } - - }; - - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); - tccb.m_hitFraction = resultCallback.m_closestHitFraction; - btVector3 boxMinLocal, boxMaxLocal; - castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); - triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); + tccb.m_hitFraction = resultCallback.m_closestHitFraction; + btVector3 boxMinLocal, boxMaxLocal; + castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); + triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); + } else + { + //currently there is no convex cast support for concave shapes other then btBvhTriangleMeshShape + //not yet + btAssert(0); + } } else { //todo: use AABB tree or other BVH acceleration structure! if (collisionShape->isCompound()) diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index ca6ada4c8..52b173702 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -265,8 +265,8 @@ public: virtual btScalar AddSingleResult(LocalConvexResult& rayResult,bool normalInWorldSpace) { //caller already does the filter on the m_closestHitFraction - assert(rayResult.m_hitFraction <= m_closestHitFraction); - + btAssert(rayResult.m_hitFraction <= m_closestHitFraction); + m_closestHitFraction = rayResult.m_hitFraction; m_hitCollisionObject = rayResult.m_hitCollisionObject; if (normalInWorldSpace) diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 3f1c2974c..5def7afea 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -754,8 +754,8 @@ void btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* bool isLeafNode; //PCK: unsigned instead of bool - unsigned BoxBoxOverlap = 0; - unsigned RayBoxOverlap = 0; + unsigned boxBoxOverlap = 0; + unsigned rayBoxOverlap = 0; btScalar lambda_max = 1.0; #define RAYAABB2 @@ -811,10 +811,10 @@ void btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* //PCK: unsigned instead of bool // only interested if this is closer than any previous hit btScalar param = 1.0; - RayBoxOverlap = 0; - BoxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + rayBoxOverlap = 0; + boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); isLeafNode = rootNode->isLeafNode(); - if (BoxBoxOverlap) + if (boxBoxOverlap) { btVector3 bounds[2]; bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); @@ -832,20 +832,19 @@ void btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* } #endif #ifdef RAYAABB2 - RayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); + rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); #else - RayBoxOverlap = btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); + rayBoxOverlap = btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); #endif } - RayBoxOverlap = true; - if (isLeafNode && RayBoxOverlap) + if (isLeafNode && rayBoxOverlap) { nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); } //PCK: unsigned instead of bool - if ((RayBoxOverlap != 0) || isLeafNode) + if ((rayBoxOverlap != 0) || isLeafNode) { rootNode++; curIndex++;