diff --git a/src/bullet/ChangeLog.txt b/src/bullet/ChangeLog.txt index 607ffceb1..234fa558a 100644 --- a/src/bullet/ChangeLog.txt +++ b/src/bullet/ChangeLog.txt @@ -1,6 +1,79 @@ Bullet Continuous Collision Detection and Physics Library Primary author and maintainer: Erwin Coumans +2007 Dec 16 + - Added btRigidBodyConstructionInfo, to make it easier to set individual setting (and leave other untouched) during rigid body construction. + Thanks Vangelis Kokkevis for pointing this out. + - Fixed memoryleak in the ConstraintDemo and Raytracer demo. + - Fixed issue with clearing forces/gravity at the end of the stepSimulation, instead of during internalSingleStepSimulation. + Thanks chunky for pointing this out: http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1780 + - Disabled additional damping in rigid body by default, but enable it in most demos. Set btRigidBodyConstructionInfo m_additionalDamping to true to enable this. + - Removed obsolete QUICKPROF BEGIN/END_PROFILE, and enabled BT_PROFILE. Profiling is enabled by default (see Bullet/Demos/OpenGL/DemoApplication.cpp how to use this). + User can switch off profiling by enabling define BT_NO_PROFILE in Bullet/src/btQuickprof.h. + +2007 Dec 14 + - Added Hello World and BulletMultiThreaded demos + - Add portable version of BulletMultiThreaded, through SequentialThreadSupport (non-parallel but sharing the same code-path) + - Add Cmake support for AllBulletDemos + + +2007 Dec 11 + - Moved the 'btRigidBody::clearForce' to the end of the stepSimulation, instead of in each substep. + See discussion http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1601 + - Added btConvexPlaneCollisionAlgorithm, makes planes perform better, and prevents tunneling + Thanks Andy O'Neil for reporting the performance/functionality issue + - Fixes for IBM Cell SDK 3.0 + Thanks to Jochen Roth for the patch. + +2007 Dec 10 + - Fixes in btHeightfieldTerrainShape + Thanks to Jay Lee for the patch. + +2007 Dec 9 + - Only update aabb of active objects + Thanks Peter Tchernev for reporting (http://bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1764 ) + - Added workaround to compile libxml under Visual Studio 2008 Beta 2 + - Make glui compile under MSVC 9.0 beta (vsnprintf is already defined) + +2007 Dec 6 + - Added DynamicControlDemo, showing dynamic control through constraint motors + Thanks to Eddy Boxerman + - Add support for generic concave shapes for convex cast. + - Added convex cast query to collision world. + - Added workaround for OpenGL bug in Mac OS X 10.5.0 (Leopard) + - Added concave raycast demo + All above thanks to John McCutchan (JMC) + - Fixed issues that prevent Linux version to compile. + Thanks to Enrico for reporting and patch, see + - Fixed misleading name 'numTriangleIndices' into 'numTriangles' + Thanks Sean Tasker for reporting: + +2007 Nov 28: + - Added raycast against trianglemesh. Will be extended to object cast soon. + Thanks John McCutchan (JMC) + - make getNumPoints const correct, add const getPoints(). + Thanks Dirk Gregorius + - Bugfix: allow btCollisionObjects (non-btRigidBody) to interact properly with btRigidBody for cache-friendly btSequentialImpulseConstraintSolver. + Thanks Andy O'Neil for pointing this out. + - Bugfix: don't fail if spheres have identical center, use arbitrary separating normal (1,0,0) + Thanks Sean Tasker for reporting! http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1681 + + +2007, November 20 + - Added hierarchical profiling + - Fixed memory leak in btMultiSapBroadphase, + - Fixed hash function (typo, should use 2 proxies) + Thanks to Stephen (shatcher) for reporting and fixes! http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1696 + +2007 Nov 11 + - Fixed parallel solver (BulletMultiThreaded) friction issue + - Terminate Win32 Threads when closing the CcdPhysicsDemo (when USE_PARALLEL_SOLVER/USE_PARALLEL_DISPATCHER is defined) + +2007 Nov 6 + - Added support for 16-bit indices for triangle meshes + - Added support for multiple mesh parts using btBvhTriangleMeshShape. + Thanks to Tim Johansson + 2007 Oct 22 - All memory allocations go through btAlignedAlloc/btAlignedFree. User can override this to verify memory leaks - added a few more demos to AllBulletDemos diff --git a/src/bullet/Demos/OpenGL/DemoApplication.cpp b/src/bullet/Demos/OpenGL/DemoApplication.cpp index 66b4ae2e7..ed25cd04a 100644 --- a/src/bullet/Demos/OpenGL/DemoApplication.cpp +++ b/src/bullet/Demos/OpenGL/DemoApplication.cpp @@ -29,6 +29,7 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "LinearMath/btDefaultMotionState.h" + #include "BMF_Api.h" extern bool gDisableDeactivation; @@ -71,12 +72,15 @@ m_shootBoxShape(0), m_singleStep(false), m_idle(false) { + m_profileIterator = CProfileManager::Get_Iterator(); } DemoApplication::~DemoApplication() { + + CProfileManager::Release_Iterator(m_profileIterator); if (m_shootBoxShape) delete m_shootBoxShape; @@ -231,8 +235,6 @@ void DemoApplication::reshape(int w, int h) } - - void DemoApplication::keyboardCallback(unsigned char key, int x, int y) { (void)x; @@ -240,9 +242,27 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y) m_lastKey = 0; + if (key >= 0x31 && key < 0x37) + { + int child = key-0x31; + m_profileIterator->Enter_Child(child); + } + if (key==0x30) + { + m_profileIterator->Enter_Parent(); + } + + switch (key) { - case 'q' : exit(0); break; + case 'q' : +#ifdef BT_USE_FREEGLUT + //return from glutMainLoop(), detect memory leaks etc. + glutLeaveMainLoop(); +#else + exit(0); +#endif + break; case 'l' : stepLeft(); break; case 'r' : stepRight(); break; @@ -469,7 +489,7 @@ void DemoApplication::shootBox(const btVector3& destination) if (m_dynamicsWorld) { - float mass = 100.f; + float mass = 10.f; btTransform startTransform; startTransform.setIdentity(); btVector3 camPos = getCameraPosition(); @@ -482,7 +502,7 @@ void DemoApplication::shootBox(const btVector3& destination) btConvexShape* childShape = new btBoxShape(btVector3(1.f,1.f,1.f)); m_shootBoxShape = new btUniformScalingShape(childShape,0.5f); #else - m_shootBoxShape = new btBoxShape(btVector3(0.5f,0.5f,0.5f)); + m_shootBoxShape = new btBoxShape(btVector3(1.f,1.f,1.f)); #endif// } @@ -716,7 +736,7 @@ btRigidBody* DemoApplication::localCreateRigidBody(float mass, const btTransform #define USE_MOTIONSTATE 1 #ifdef USE_MOTIONSTATE btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); - btRigidBody* body = new btRigidBody(mass,myMotionState,shape,localInertia); + btRigidBody* body = new btRigidBody(btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,shape,localInertia)); #else btRigidBody* body = new btRigidBody(mass,0,shape,localInertia); @@ -756,6 +776,88 @@ void DemoApplication::resetPerspectiveProjection() } + + +extern CProfileIterator * m_profileIterator; + +void DemoApplication::displayProfileString(int xOffset,int yStart,char* message) +{ + glRasterPos3f(xOffset,yStart,0); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),message); +} + + +void DemoApplication::showProfileInfo(float& xOffset,float& yStart, float yIncr) +{ + + static double time_since_reset = 0.f; + if (!m_idle) + { + time_since_reset = CProfileManager::Get_Time_Since_Reset(); + } + + + { + //recompute profiling data, and store profile strings + + char blockTime[128]; + + double totalTime = 0; + + int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); + + m_profileIterator->First(); + + double parent_time = m_profileIterator->Is_Root() ? time_since_reset : m_profileIterator->Get_Current_Parent_Total_Time(); + + { + sprintf(blockTime,"--- Profiling: %s (total running time: %.3f ms) ---", m_profileIterator->Get_Current_Parent_Name(), parent_time ); + displayProfileString(xOffset,yStart,blockTime); + yStart += yIncr; + sprintf(blockTime,"press number (1,2...) to display child timings, or 0 to go up to parent" ); + displayProfileString(xOffset,yStart,blockTime); + yStart += yIncr; + + } + + + double accumulated_time = 0.f; + + for (int i = 0; !m_profileIterator->Is_Done(); m_profileIterator->Next()) + { + double current_total_time = m_profileIterator->Get_Current_Total_Time(); + accumulated_time += current_total_time; + double fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f; + + sprintf(blockTime,"%d -- %s (%.2f %%) :: %.3f ms / frame (%d calls)", + ++i, m_profileIterator->Get_Current_Name(), fraction, + (current_total_time / (double)frames_since_reset),m_profileIterator->Get_Current_Total_Calls()); + displayProfileString(xOffset,yStart,blockTime); + yStart += yIncr; + totalTime += current_total_time; + } + + sprintf(blockTime,"%s (%.3f %%) :: %.3f ms", "Unaccounted", + // (min(0, time_since_reset - totalTime) / time_since_reset) * 100); + parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); + + displayProfileString(xOffset,yStart,blockTime); + yStart += yIncr; + + + + sprintf(blockTime,"-------------------------------------------------"); + displayProfileString(xOffset,yStart,blockTime); + yStart += yIncr; + + } + + + + +} + + void DemoApplication::renderme() { updateCamera(); @@ -823,6 +925,8 @@ void DemoApplication::renderme() { setOrthographicProjection(); + showProfileInfo(xOffset,yStart,yIncr); + #ifdef USE_QUICKPROF diff --git a/src/bullet/Demos/OpenGL/DemoApplication.h b/src/bullet/Demos/OpenGL/DemoApplication.h index 0c4585b54..8327b95f8 100644 --- a/src/bullet/Demos/OpenGL/DemoApplication.h +++ b/src/bullet/Demos/OpenGL/DemoApplication.h @@ -17,18 +17,7 @@ subject to the following restrictions: #define DEMO_APPLICATION_H -#ifdef WIN32//for glut.h -#include -#endif - -//think different -#if defined(__APPLE__) && !defined (VMDMESA) -#include -#include -#include -#else -#include -#endif +#include "GlutStuff.h" #include #include @@ -39,15 +28,21 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btQuickprof.h" +#include "LinearMath/btAlignedObjectArray.h" class btCollisionShape; class btDynamicsWorld; class btRigidBody; class btTypedConstraint; + + + class DemoApplication { - + void displayProfileString(int xOffset,int yStart,char* message); + class CProfileIterator* m_profileIterator; + protected: btClock m_clock; @@ -82,7 +77,10 @@ class DemoApplication bool m_singleStep; bool m_idle; int m_lastKey; - + + void showProfileInfo(float& xOffset,float& yStart, float yIncr); + + public: DemoApplication(); diff --git a/src/bullet/Demos/OpenGL/GlutStuff.cpp b/src/bullet/Demos/OpenGL/GlutStuff.cpp index 65a603aea..e06a1dd58 100644 --- a/src/bullet/Demos/OpenGL/GlutStuff.cpp +++ b/src/bullet/Demos/OpenGL/GlutStuff.cpp @@ -66,7 +66,6 @@ static void glutDisplayCallback(void) } - int glutmain(int argc, char **argv,int width,int height,const char* title,DemoApplication* demoApp) { gDemoApplication = demoApp; @@ -76,7 +75,10 @@ int glutmain(int argc, char **argv,int width,int height,const char* title,DemoAp glutInitWindowPosition(0, 0); glutInitWindowSize(width, height); glutCreateWindow(title); - +#ifdef BT_USE_FREEGLUT + glutSetOption (GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_GLUTMAINLOOP_RETURNS); +#endif + gDemoApplication->myinit(); glutKeyboardFunc(glutKeyboardCallback); diff --git a/src/bullet/Demos/OpenGL/GlutStuff.h b/src/bullet/Demos/OpenGL/GlutStuff.h index 1ee2ba7d1..4727845bc 100644 --- a/src/bullet/Demos/OpenGL/GlutStuff.h +++ b/src/bullet/Demos/OpenGL/GlutStuff.h @@ -15,6 +15,24 @@ subject to the following restrictions: #ifndef GLUT_STUFF_H #define GLUT_STUFF_H +#ifdef WIN32//for glut.h +#include +#endif + +//think different +#if defined(__APPLE__) && !defined (VMDMESA) +#include +#include +#include +#else +#include +#endif + +#if BT_USE_FREEGLUT +#include "GL/freeglut_ext.h" //to be able to return from glutMainLoop() +#endif + + class DemoApplication; int glutmain(int argc, char **argv,int width,int height,const char* title,DemoApplication* demoApp); diff --git a/src/bullet/VERSION b/src/bullet/VERSION index 43c134489..b53eaca55 100644 --- a/src/bullet/VERSION +++ b/src/bullet/VERSION @@ -1,3 +1,3 @@ -Bullet Collision Detection and Physics Library version 2.64 +Bullet Collision Detection and Physics Library version 2.66 http://bullet.sourceforge.net diff --git a/src/bullet/copy_bullet_files b/src/bullet/copy_bullet_files new file mode 100755 index 000000000..9a7ec4bd3 --- /dev/null +++ b/src/bullet/copy_bullet_files @@ -0,0 +1,28 @@ +LOCATION=$1 +cp $LOCATION/AUTHORS . +cp $LOCATION/BulletLicense.txt . +cp $LOCATION/ChangeLog.txt . +cp $LOCATION/LICENSE . +cp $LOCATION/Makefile.am . +cp $LOCATION/README . +cp $LOCATION/VERSION . +cp $LOCATION/src/*.h src +cp $LOCATION/src/LinearMath/*.h src/LinearMath/ +cp $LOCATION/src/LinearMath/*.cpp src/LinearMath/ +cp $LOCATION/src/BulletCollision/BroadphaseCollision/*.h src/BulletCollision/BroadphaseCollision +cp $LOCATION/src/BulletCollision/BroadphaseCollision/*.cpp src/BulletCollision/BroadphaseCollision +cp $LOCATION/src/BulletCollision/CollisionDispatch/*.h src/BulletCollision/CollisionDispatch +cp $LOCATION/src/BulletCollision/CollisionDispatch/*.cpp src/BulletCollision/CollisionDispatch +cp $LOCATION/src/BulletCollision/CollisionShapes/*.h src/BulletCollision/CollisionShapes +cp $LOCATION/src/BulletCollision/CollisionShapes/*.cpp src/BulletCollision/CollisionShapes +cp $LOCATION/src/BulletCollision/NarrowPhaseCollision/*.h src/BulletCollision/NarrowPhaseCollision +cp $LOCATION/src/BulletCollision/NarrowPhaseCollision/*.cpp src/BulletCollision/NarrowPhaseCollision +cp $LOCATION/src/BulletDynamics/ConstraintSolver/*.h src/BulletDynamics/ConstraintSolver +cp $LOCATION/src/BulletDynamics/ConstraintSolver/*.cpp src/BulletDynamics/ConstraintSolver +cp $LOCATION/src/BulletDynamics/Dynamics/*.h src/BulletDynamics/Dynamics +cp $LOCATION/src/BulletDynamics/Dynamics/*.cpp src/BulletDynamics/Dynamics +cp $LOCATION/src/BulletDynamics/Vehicle/*.h src/BulletDynamics/Vehicle +cp $LOCATION/src/BulletDynamics/Vehicle/*.cpp src/BulletDynamics/Vehicle +cp $LOCATION/Demos/OpenGL/*.h Demos/OpenGL +cp $LOCATION/Demos/OpenGL/*.cpp Demos/OpenGL + diff --git a/src/bullet/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/src/bullet/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp index a75689c99..2799b2248 100644 --- a/src/bullet/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp +++ b/src/bullet/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp @@ -80,7 +80,7 @@ btBroadphaseProxy* btMultiSapBroadphase::createProxy( const btVector3& aabbMin, simpleProxy->m_multiSapParentProxy = proxy; mem = btAlignedAlloc(sizeof(btChildProxy),16); - btChildProxy* childProxyRef = new btChildProxy(); + btChildProxy* childProxyRef = new(mem) btChildProxy(); childProxyRef->m_proxy = simpleProxy; childProxyRef->m_childBroadphase = m_simpleBroadphase; proxy->m_childProxies.push_back(childProxyRef); diff --git a/src/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 8930dbaa9..c3a9339e0 100644 --- a/src/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/src/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -177,7 +177,7 @@ private: SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) { - int key = ((unsigned int)proxyId1) | (((unsigned int)proxyId1) <<16); + int key = ((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16); // Thomas Wang's hash key += ~(key << 15); diff --git a/src/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/src/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index e0bb09929..1e5fe0d1b 100644 --- a/src/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/src/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -14,8 +14,8 @@ subject to the following restrictions: */ #include "btSimpleBroadphase.h" -#include -#include +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 644caf267..0f3a129e2 100644 --- a/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -35,6 +35,7 @@ int gNumManifold = 0; btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): m_count(0), m_useIslands(true), + m_staticWarningReported(false), m_collisionConfiguration(collisionConfiguration) { int i; @@ -117,7 +118,7 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) manifold->~btPersistentManifold(); if (m_persistentManifoldPoolAllocator->validPtr(manifold)) { - m_persistentManifoldPoolAllocator->free(manifold); + m_persistentManifoldPoolAllocator->freeMemory(manifold); } else { btAlignedFree(manifold); @@ -161,13 +162,19 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO bool needsCollision = true; - //broadphase filtering already deals with this - if ((body0->isStaticObject() || body0->isKinematicObject()) && - (body1->isStaticObject() || body1->isKinematicObject())) +#ifdef BT_DEBUG + if (!m_staticWarningReported) { - printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); + //broadphase filtering already deals with this + if ((body0->isStaticObject() || body0->isKinematicObject()) && + (body1->isStaticObject() || body1->isKinematicObject())) + { + m_staticWarningReported = true; + printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); + } } - +#endif //BT_DEBUG + if ((!body0->isActive()) && (!body1->isActive())) needsCollision = false; else if (!body0->checkCollideWith(body1)) @@ -279,7 +286,7 @@ void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) { if (m_collisionAlgorithmPoolAllocator->validPtr(ptr)) { - m_collisionAlgorithmPoolAllocator->free(ptr); + m_collisionAlgorithmPoolAllocator->freeMemory(ptr); } else { btAlignedFree(ptr); diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 45aaa1bd9..b6b57e8ce 100644 --- a/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -47,6 +47,8 @@ class btCollisionDispatcher : public btDispatcher btAlignedObjectArray m_manifoldsPtr; bool m_useIslands; + + bool m_staticWarningReported; btManifoldResult m_defaultManifoldResult; diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 6b72a131c..12267e2a9 100644 --- a/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -19,11 +19,13 @@ subject to the following restrictions: btCollisionObject::btCollisionObject() : m_broadphaseHandle(0), m_collisionShape(0), - m_collisionFlags(0), + m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), m_islandTag1(-1), m_companionId(-1), m_activationState1(1), m_deactivationTime(btScalar(0.)), + m_friction(btScalar(0.5)), + m_restitution(btScalar(0.)), m_userObjectPointer(0), m_internalOwner(0), m_hitFraction(btScalar(1.)), diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index b4828508b..5e5249d8f 100644 --- a/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" @@ -32,6 +32,7 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "LinearMath/btStackAlloc.h" + //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor) #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" @@ -40,7 +41,8 @@ subject to the following restrictions: btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) :m_dispatcher1(dispatcher), -m_broadphasePairCache(pairCache) +m_broadphasePairCache(pairCache), +m_debugDrawer(0) { m_stackAlloc = collisionConfiguration->getStackAllocator(); m_dispatchInfo.m_stackAllocator = m_stackAlloc; @@ -111,36 +113,68 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho } +void btCollisionWorld::updateAabbs() +{ + BT_PROFILE("updateAabbs"); + + btTransform predictedTrans; + for ( int i=0;iisActive()) + { + btPoint3 minAabb,maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; + + //moving objects should be moderately sized, probably something wrong if not + if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) + { + bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); + } else + { + //something went wrong, investigate + //this assert is unwanted in 3D modelers (danger of loosing work) + colObj->setActivationState(DISABLE_SIMULATION); + + static bool reportMe = true; + if (reportMe && m_debugDrawer) + { + reportMe = false; + m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); + m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); + m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); + m_debugDrawer->reportErrorWarning("Thanks.\n"); + } + } + } + } + +} void btCollisionWorld::performDiscreteCollisionDetection() { + BT_PROFILE("performDiscreteCollisionDetection"); + btDispatcherInfo& dispatchInfo = getDispatchInfo(); - BEGIN_PROFILE("perform Broadphase Collision Detection"); + updateAabbs(); - - //update aabb (of all moved objects) - - btVector3 aabbMin,aabbMax; - for (int i=0;igetCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax); - m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax,m_dispatcher1); + m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); } - m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); - - END_PROFILE("perform Broadphase Collision Detection"); - - BEGIN_PROFILE("performDiscreteCollisionDetection"); btDispatcher* dispatcher = getDispatcher(); - if (dispatcher) - dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); - - END_PROFILE("performDiscreteCollisionDetection"); + { + BT_PROFILE("dispatchAllCollisionPairs"); + if (dispatcher) + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); + } } @@ -180,155 +214,367 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra const btTransform& colObjWorldTransform, RayResultCallback& resultCallback,short int collisionFilterMask) { - btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); - - objectQuerySingle(&pointShape,rayFromTrans,rayToTrans, - collisionObject, - collisionShape, - colObjWorldTransform, - resultCallback,collisionFilterMask); -} - -void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback,short int collisionFilterMask) -{ - + const btConvexShape* castShape = &pointShape; if (collisionShape->isConvex()) - { - btConvexCast::CastResult castResult; - castResult.m_fraction = btScalar(1.);//?? + { + btConvexCast::CastResult castResult; + castResult.m_fraction = resultCallback.m_closestHitFraction; - btConvexShape* convexShape = (btConvexShape*) collisionShape; - btVoronoiSimplexSolver simplexSolver; -#define USE_SUBSIMPLEX_CONVEX_CAST 1 + btConvexShape* convexShape = (btConvexShape*) collisionShape; + btVoronoiSimplexSolver simplexSolver; +#define USE_SUBSIMPLEX_CONVEX_CAST 1 #ifdef USE_SUBSIMPLEX_CONVEX_CAST btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver); #else - //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); - //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); + //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); + //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); #endif //#USE_SUBSIMPLEX_CONVEX_CAST - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < resultCallback.m_closestHitFraction) { - //add hit - if (castResult.m_normal.length2() > btScalar(0.0001)) - { - - if (castResult.m_fraction < resultCallback.m_closestHitFraction) - { #ifdef USE_SUBSIMPLEX_CONVEX_CAST - //rotate normal into worldspace - castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; + //rotate normal into worldspace + castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; #endif //USE_SUBSIMPLEX_CONVEX_CAST - castResult.m_normal.normalize(); - btCollisionWorld::LocalRayResult localRayResult + castResult.m_normal.normalize(); + btCollisionWorld::LocalRayResult localRayResult + ( + collisionObject, + 0, + castResult.m_normal, + castResult.m_fraction + ); + + bool normalInWorldSpace = true; + resultCallback.AddSingleResult(localRayResult, normalInWorldSpace); + + } + } + } + } else { + if (collisionShape->isConcave()) + { + if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + ///optimized version for btBvhTriangleMeshShape + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + //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); + } + + }; + + 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::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); + + + } + + }; + + + 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()) + { + const btCompoundShape* compoundShape = static_cast(collisionShape); + int i=0; + for (i=0;igetNumChildShapes();i++) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childWorldTrans = colObjWorldTransform * childTrans; + rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback, collisionFilterMask); + + } + } + } + } +} + +void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback,short int collisionFilterMask) +{ + if (collisionShape->isConvex()) + { + btConvexCast::CastResult castResult; + castResult.m_fraction = btScalar(1.);//?? + + btConvexShape* convexShape = (btConvexShape*) collisionShape; + btVoronoiSimplexSolver simplexSolver; + btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); + + if (convexCaster.calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < resultCallback.m_closestHitFraction) + { +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + //rotate normal into worldspace + castResult.m_normal = convexFromTrans.getBasis() * castResult.m_normal; +#endif //USE_SUBSIMPLEX_CONVEX_CAST + + castResult.m_normal.normalize(); + btCollisionWorld::LocalConvexResult localConvexResult ( collisionObject, 0, castResult.m_normal, + castResult.m_hitPoint, castResult.m_fraction ); - bool normalInWorldSpace = true; - resultCallback.AddSingleResult(localRayResult, normalInWorldSpace); + bool normalInWorldSpace = true; + resultCallback.AddSingleResult(localConvexResult, normalInWorldSpace); - } - } } } - else + } + } else { + if (collisionShape->isConcave()) + { + if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { - - if (collisionShape->isConcave()) - { + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); + btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); + // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation + btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); - btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape; - - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + //ConvexCast::CastResult + struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback + { + btCollisionWorld::ConvexResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; - btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); - btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); - - //ConvexCast::CastResult - - struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + 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) { - 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); - - - } - - }; - - - 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()) - { - const btCompoundShape* compoundShape = static_cast(collisionShape); - int i=0; - for (i=0;igetNumChildShapes();i++) - { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); - btTransform childWorldTrans = colObjWorldTransform * childTrans; - objectQuerySingle(castShape, rayFromTrans,rayToTrans, - collisionObject, - childCollisionShape, - childWorldTrans, - resultCallback, collisionFilterMask); - - } - - } + + + 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; } + + }; + + 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 + { + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); + btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); + // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation + btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); + + //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; + } + + }; + + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); + tccb.m_hitFraction = resultCallback.m_closestHitFraction; + btVector3 boxMinLocal, boxMaxLocal; + castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); + + btVector3 rayAabbMinLocal = convexFromLocal; + rayAabbMinLocal.setMin(convexToLocal); + btVector3 rayAabbMaxLocal = convexFromLocal; + rayAabbMaxLocal.setMax(convexToLocal); + rayAabbMinLocal += boxMinLocal; + rayAabbMaxLocal += boxMaxLocal; + triangleMesh->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); } + } else { + //todo: use AABB tree or other BVH acceleration structure! + if (collisionShape->isCompound()) + { + const btCompoundShape* compoundShape = static_cast(collisionShape); + int i=0; + for (i=0;igetNumChildShapes();i++) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childWorldTrans = colObjWorldTransform * childTrans; + objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback, collisionFilterMask); + } + } + } + } } void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback,short int collisionFilterMask) @@ -347,6 +593,10 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r int i; for (i=0;igetBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) { @@ -354,7 +604,7 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r btVector3 collisionObjectAabbMin,collisionObjectAabbMax; collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btScalar hitLambda = resultCallback.m_closestHitFraction; btVector3 hitNormal; if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) { @@ -365,6 +615,46 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r resultCallback); } } + + } + +} + +void btCollisionWorld::convexTest(const btConvexShape* castShape, const btVector3& convexFromWorld, const btVector3& convexToWorld, ConvexResultCallback& resultCallback,short int collisionFilterMask) +{ + btTransform convexFromTrans,convexToTrans; + convexFromTrans.setIdentity(); + convexFromTrans.setOrigin(convexFromWorld); + convexToTrans.setIdentity(); + convexToTrans.setOrigin(convexToWorld); + btVector3 castShapeAabbMin, castShapeAabbMax; + btTransform I; + I.setIdentity(); + castShape->getAabb (I, castShapeAabbMin, castShapeAabbMax); + + /// go over all objects, and if the ray intersects their aabb + cast shape aabb, + // do a ray-shape query using convexCaster (CCD) + int i; + for (i=0;igetBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(convexFromWorld,convexToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + { + objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); + } + } } } diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index bda03cced..5980f0293 100644 --- a/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -91,6 +91,9 @@ protected: btStackAlloc* m_stackAlloc; btBroadphaseInterface* m_broadphasePairCache; + + btIDebugDraw* m_debugDrawer; + public: @@ -116,6 +119,19 @@ public: return m_dispatcher1; } + virtual void updateAabbs(); + + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) + { + m_debugDrawer = debugDrawer; + } + + virtual btIDebugDraw* getDebugDrawer() + { + return m_debugDrawer; + } + + ///LocalShapeInfo gives extra information for complex shapes ///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart struct LocalShapeInfo @@ -150,17 +166,20 @@ public: ///RayResultCallback is used to report new raycast results struct RayResultCallback { + btScalar m_closestHitFraction; + btCollisionObject* m_collisionObject; + virtual ~RayResultCallback() { } - btScalar m_closestHitFraction; bool HasHit() { - return (m_closestHitFraction < btScalar(1.)); + return (m_collisionObject != 0); } RayResultCallback() - :m_closestHitFraction(btScalar(1.)) + :m_closestHitFraction(btScalar(1.)), + m_collisionObject(0) { } virtual btScalar AddSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0; @@ -170,8 +189,7 @@ public: { ClosestRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_collisionObject(0) + m_rayToWorld(rayToWorld) { } @@ -180,13 +198,11 @@ public: btVector3 m_hitNormalWorld; btVector3 m_hitPointWorld; - btCollisionObject* m_collisionObject; - + virtual btScalar AddSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) { - -//caller already does the filter on the m_closestHitFraction - assert(rayResult.m_hitFraction <= m_closestHitFraction); + //caller already does the filter on the m_closestHitFraction + btAssert(rayResult.m_hitFraction <= m_closestHitFraction); m_closestHitFraction = rayResult.m_hitFraction; m_collisionObject = rayResult.m_collisionObject; @@ -204,7 +220,83 @@ public: }; - + struct LocalConvexResult + { + LocalConvexResult(btCollisionObject* hitCollisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + const btVector3& hitPointLocal, + btScalar hitFraction + ) + :m_hitCollisionObject(hitCollisionObject), + m_localShapeInfo(localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitPointLocal(hitPointLocal), + m_hitFraction(hitFraction) + { + } + + btCollisionObject* m_hitCollisionObject; + LocalShapeInfo* m_localShapeInfo; + btVector3 m_hitNormalLocal; + btVector3 m_hitPointLocal; + btScalar m_hitFraction; + }; + + ///RayResultCallback is used to report new raycast results + struct ConvexResultCallback + { + virtual ~ConvexResultCallback() + { + } + btScalar m_closestHitFraction; + bool HasHit() + { + return (m_closestHitFraction < btScalar(1.)); + } + + ConvexResultCallback() + :m_closestHitFraction(btScalar(1.)) + { + } + virtual btScalar AddSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0; + }; + + struct ClosestConvexResultCallback : public ConvexResultCallback + { + ClosestConvexResultCallback(const btVector3& convexFromWorld,const btVector3& convexToWorld) + :m_convexFromWorld(convexFromWorld), + m_convexToWorld(convexToWorld), + m_hitCollisionObject(0) + { + } + + btVector3 m_convexFromWorld;//used to calculate hitPointWorld from hitFraction + btVector3 m_convexToWorld; + + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; + btCollisionObject* m_hitCollisionObject; + + virtual btScalar AddSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) + { +//caller already does the filter on the m_closestHitFraction + btAssert(convexResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = convexResult.m_hitFraction; + m_hitCollisionObject = convexResult.m_hitCollisionObject; + if (normalInWorldSpace) + { + m_hitNormalWorld = convexResult.m_hitNormalLocal; + } else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; + } + m_hitPointWorld = convexResult.m_hitPointLocal; + return convexResult.m_hitFraction; + } + }; int getNumCollisionObjects() const { @@ -215,6 +307,10 @@ public: /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback, short int collisionFilterMask=-1); + // convexTest performs a linear convex cast on all objects in the btCollisionWorld, and calls the resultCallback + // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. + void convexTest (const btConvexShape* castShape, const btVector3& from, const btVector3& to, ConvexResultCallback& resultCallback, short int collisionFilterMask=-1); + /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. /// This allows more customization. @@ -229,7 +325,7 @@ public: btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback, short int collisionFilterMask=-1); + ConvexResultCallback& resultCallback, short int collisionFilterMask=-1); void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=1,short int collisionFilterMask=1); diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/src/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp new file mode 100644 index 000000000..5b16a5e7e --- /dev/null +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -0,0 +1,108 @@ +/* +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 "btConvexPlaneCollisionAlgorithm.h" + +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" + +//#include + +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) +: btCollisionAlgorithm(ci), +m_ownManifold(false), +m_manifoldPtr(mf), +m_isSwapped(isSwapped) +{ + btCollisionObject* convexObj = m_isSwapped? col1 : col0; + btCollisionObject* planeObj = m_isSwapped? col0 : col1; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj); + m_ownManifold = true; + } +} + + +btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + + + +void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + (void)resultOut; + if (!m_manifoldPtr) + return; + + btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* planeObj = m_isSwapped? body0: body1; + + btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + btTransform planeInConvex; + planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform(); + btTransform convexInPlaneTrans; + convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertexWithoutMargin(planeInConvex.getBasis()*-planeNormal); + btVector3 vtxInPlane = convexInPlaneTrans(vtx); + btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant) - convexShape->getMargin(); + + btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected; + + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + resultOut->setPersistentManifold(m_manifoldPtr); + if (hasCollision) + { + /// report a contact. internally this will be kept persistent, and contact reduction is done + btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal; + btVector3 pOnB = vtxInPlaneWorld; + resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + } + if (m_ownManifold) + { + if (m_manifoldPtr->getNumContacts()) + { + resultOut->refreshContactPoints(); + } + } +} + +btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/src/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h new file mode 100644 index 000000000..b503ba40c --- /dev/null +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -0,0 +1,63 @@ +/* +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. +*/ + +#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H +#define CONVEX_PLANE_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" + +/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; + +public: + + btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); + + virtual ~btConvexPlaneCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); + if (!m_swapped) + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false); + } else + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true); + } + } + }; + +}; + +#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H + diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/src/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 661270b9b..83286c37a 100644 --- a/src/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -20,6 +20,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h" + #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" @@ -58,7 +60,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* s m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16); m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); @@ -72,7 +74,13 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* s m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; m_triangleSphereCF->m_swapped = true; - + //convex versus plane + mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; + m_planeConvexCF->m_swapped = true; + ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool int maxSize = sizeof(btConvexConvexAlgorithm); int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); @@ -167,6 +175,11 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() m_triangleSphereCF->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_triangleSphereCF); + m_convexPlaneCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexPlaneCF); + m_planeConvexCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_planeConvexCF); + m_simplexSolver->~btVoronoiSimplexSolver(); btAlignedFree(m_simplexSolver); m_pdSolver->~btGjkEpaPenetrationDepthSolver(); @@ -178,7 +191,8 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) { - + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) { @@ -205,6 +219,15 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg return m_triangleSphereCF; } + if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) + { + return m_convexPlaneCF; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE)) + { + return m_planeConvexCF; + } if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) { diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/src/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 2e99f1db1..14475ad14 100644 --- a/src/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -54,7 +54,9 @@ class btDefaultCollisionConfiguration : public btCollisionConfiguration btCollisionAlgorithmCreateFunc* m_boxSphereCF; btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; btCollisionAlgorithmCreateFunc* m_triangleSphereCF; - + btCollisionAlgorithmCreateFunc* m_planeConvexCF; + btCollisionAlgorithmCreateFunc* m_convexPlaneCF; + public: btDefaultCollisionConfiguration(btStackAlloc* stackAlloc=0,btPoolAllocator* persistentManifoldPool=0,btPoolAllocator* collisionAlgorithmPool=0); diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 0cc3ac599..d8239127e 100644 --- a/src/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -138,7 +138,7 @@ class btPersistentManifoldSortPredicate void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback) { - BEGIN_PROFILE("islandUnionFindAndHeapSort"); + BT_PROFILE("islandUnionFindAndHeapSort"); //we are going to sort the unionfind array, and store the element id in the size //afterwards, we clean unionfind, to make sure no-one uses it anymore @@ -295,8 +295,6 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, //int islandId; - END_PROFILE("islandUnionFindAndHeapSort"); - // printf("Start Islands\n"); diff --git a/src/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/src/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index 45960cc76..2c2d78455 100644 --- a/src/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/src/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -66,7 +66,12 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 ///distance (negative means penetration) btScalar dist = len - (radius0+radius1); - btVector3 normalOnSurfaceB = diff / len; + btVector3 normalOnSurfaceB(1,0,0); + if (len > SIMD_EPSILON) + { + normalOnSurfaceB = diff / len; + } + ///point on A (worldspace) btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/src/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index eea263fe5..e8e0347e9 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/src/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -18,7 +18,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btOptimizedBvh.h" - ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh) @@ -92,6 +91,126 @@ btBvhTriangleMeshShape::~btBvhTriangleMeshShape() } } +void btBvhTriangleMeshShape::performRaycast (btTriangleRaycastCallback* callback, const btVector3& raySource, const btVector3& rayTarget) +{ + struct MyNodeOverlapCallback : public btNodeOverlapCallback + { + btStridingMeshInterface* m_meshInterface; + btTriangleRaycastCallback* m_callback; + + MyNodeOverlapCallback(btTriangleRaycastCallback* callback,btStridingMeshInterface* meshInterface) + :m_meshInterface(meshInterface), + m_callback(callback) + { + } + + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) + { + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + nodeSubPart); + + int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); + + const btVector3& meshScaling = m_meshInterface->getScaling(); + for (int j=2;j>=0;j--) + { + int graphicsindex = indicestype==PHY_SHORT?((short*)gfxbase)[j]:gfxbase[j]; + + btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + } + + /* Perform ray vs. triangle collision here */ + m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); + } + }; + + MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + + m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget); +} + +void btBvhTriangleMeshShape::performConvexcast (btTriangleConvexcastCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) +{ + struct MyNodeOverlapCallback : public btNodeOverlapCallback + { + btStridingMeshInterface* m_meshInterface; + btTriangleConvexcastCallback* m_callback; + + MyNodeOverlapCallback(btTriangleConvexcastCallback* callback,btStridingMeshInterface* meshInterface) + :m_meshInterface(meshInterface), + m_callback(callback) + { + } + + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) + { + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + nodeSubPart); + + int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); + + const btVector3& meshScaling = m_meshInterface->getScaling(); + for (int j=2;j>=0;j--) + { + int graphicsindex = indicestype==PHY_SHORT?((short*)gfxbase)[j]:gfxbase[j]; + + btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + } + + /* Perform ray vs. triangle collision here */ + m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); + } + }; + + MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + + m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); +} + //perform bvh tree traversal and report overlapping triangles to 'callback' void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { @@ -141,12 +260,13 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co nodeSubPart); int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j=2;j>=0;j--) { - int graphicsindex = gfxbase[j]; + int graphicsindex = indicestype==PHY_SHORT?((short*)gfxbase)[j]:gfxbase[j]; #ifdef DEBUG_TRIANGLE_MESH diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/src/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 95c73b244..53500c37b 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/src/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -19,6 +19,7 @@ subject to the following restrictions: #include "btTriangleMeshShape.h" #include "btOptimizedBvh.h" #include "LinearMath/btAlignedAllocator.h" +#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. @@ -27,7 +28,7 @@ ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape btOptimizedBvh* m_bvh; bool m_useQuantizedAabbCompression; - bool m_ownsBvh; + bool m_ownsBvh; bool m_pad[11];////need padding due to alignment public: @@ -42,15 +43,13 @@ public: virtual ~btBvhTriangleMeshShape(); - - /* virtual int getShapeType() const { return TRIANGLE_MESH_SHAPE_PROXYTYPE; } - */ - - + + void performRaycast (btTriangleRaycastCallback* callback, const btVector3& raySource, const btVector3& rayTarget); + void performConvexcast (btTriangleConvexcastCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax); virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/src/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h index 0928d68b8..abde7862b 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/src/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -44,7 +44,12 @@ public: return &m_points[0]; } - int getNumPoints() + const btPoint3* getPoints() const + { + return &m_points[0]; + } + + int getNumPoints() const { return m_points.size(); } diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index e48757775..872041869 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/src/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -18,9 +18,11 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" -btHeightfieldTerrainShape::btHeightfieldTerrainShape(int width,int length,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) -:m_width(width), -m_length(length), +btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) +: m_heightStickWidth(heightStickWidth), +m_heightStickLength(heightStickLength), +m_width((btScalar)heightStickWidth-1), +m_length((btScalar)heightStickLength-1), m_maxHeight(maxHeight), m_heightfieldDataUnknown(heightfieldData), m_useFloatData(useFloatData), @@ -44,7 +46,7 @@ m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) { halfExtents.setValue( btScalar(m_maxHeight), - btScalar(m_width), + btScalar(m_width), //?? don't know if this should change btScalar(m_length)); break; } @@ -114,11 +116,11 @@ btScalar btHeightfieldTerrainShape::getHeightFieldValue(int x,int y) const btScalar val = 0.f; if (m_useFloatData) { - val = m_heightfieldDataFloat[(y*m_width)+x]; + val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; } else { //assume unsigned short int - unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_width)+x]; + unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; val = heightFieldValue* (m_maxHeight/btScalar(65535)); } return val; @@ -133,8 +135,8 @@ void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const btAssert(x>=0); btAssert(y>=0); - btAssert(xstartX) startX = quantizedAabbMin[1]; @@ -243,10 +245,10 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback } case 1: { - quantizedAabbMin[0]+=m_width/2-1; - quantizedAabbMax[0]+=m_width/2+1; - quantizedAabbMin[2]+=m_length/2-1; - quantizedAabbMax[2]+=m_length/2+1; + quantizedAabbMin[0]+=m_heightStickWidth/2-1; + quantizedAabbMax[0]+=m_heightStickWidth/2+1; + quantizedAabbMin[2]+=m_heightStickLength/2-1; + quantizedAabbMax[2]+=m_heightStickLength/2+1; if (quantizedAabbMin[0]>startX) startX = quantizedAabbMin[0]; @@ -260,10 +262,10 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback }; case 2: { - quantizedAabbMin[0]+=m_width/2-1; - quantizedAabbMax[0]+=m_width/2+1; - quantizedAabbMin[1]+=m_length/2-1; - quantizedAabbMax[1]+=m_length/2+1; + quantizedAabbMin[0]+=m_heightStickWidth/2-1; + quantizedAabbMax[0]+=m_heightStickWidth/2+1; + quantizedAabbMin[1]+=m_heightStickLength/2-1; + quantizedAabbMax[1]+=m_heightStickLength/2+1; if (quantizedAabbMin[0]>startX) startX = quantizedAabbMin[0]; @@ -290,7 +292,7 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback for(int x=startX; x aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; - return overlap; - } - -public: - btHeightfieldTerrainShape(int width,int height,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); - - virtual ~btHeightfieldTerrainShape(); - - - void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} - - virtual int getShapeType() const - { - return TERRAIN_SHAPE_PROXYTYPE; - } - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void setLocalScaling(const btVector3& scaling); - - virtual const btVector3& getLocalScaling() const; - - //debugging - virtual const char* getName()const {return "HEIGHTFIELD";} - -}; - -#endif //HEIGHTFIELD_TERRAIN_SHAPE_H +/* +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. +*/ + +#ifndef HEIGHTFIELD_TERRAIN_SHAPE_H +#define HEIGHTFIELD_TERRAIN_SHAPE_H + +#include "btConcaveShape.h" + +///btHeightfieldTerrainShape simulates a 2D heightfield terrain +class btHeightfieldTerrainShape : public btConcaveShape +{ +protected: + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + + ///terrain data + int m_heightStickWidth; + int m_heightStickLength; + btScalar m_maxHeight; + btScalar m_width; + btScalar m_length; + union + { + unsigned char* m_heightfieldDataUnsignedChar; + btScalar* m_heightfieldDataFloat; + void* m_heightfieldDataUnknown; + }; + + bool m_useFloatData; + bool m_flipQuadEdges; + bool m_useDiamondSubdivision; + + int m_upAxis; + + btVector3 m_localScaling; + + virtual btScalar getHeightFieldValue(int x,int y) const; + void quantizeWithClamp(int* out, const btVector3& point) const; + void getVertex(int x,int y,btVector3& vertex) const; + + inline bool testQuantizedAabbAgainstQuantizedAabb(int* aabbMin1, int* aabbMax1,const int* aabbMin2,const int* aabbMax2) const + { + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; + } + +public: + btHeightfieldTerrainShape(int heightStickWidth,int heightStickHeight,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); + + virtual ~btHeightfieldTerrainShape(); + + + void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} + + virtual int getShapeType() const + { + return TERRAIN_SHAPE_PROXYTYPE; + } + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + virtual void setLocalScaling(const btVector3& scaling); + + virtual const btVector3& getLocalScaling() const; + + //debugging + virtual const char* getName()const {return "HEIGHTFIELD";} + +}; + +#endif //HEIGHTFIELD_TERRAIN_SHAPE_H diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/src/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index a8ce8622f..c709205ef 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/src/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -47,7 +47,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) + if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON)) { vec.setValue(1,0,0); } else diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index e80469f45..c608e7bb4 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/src/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -12,6 +12,7 @@ subject to the following restrictions: 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 "btOptimizedBvh.h" #include "btStridingMeshInterface.h" #include "LinearMath/btAabbUtil2.h" @@ -95,7 +96,9 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) { - btAssert(partId==0); + // The partId and triangle index must fit in the same (positive) integer + btAssert(partId < (1<=0); @@ -132,7 +135,7 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMin[0],aabbMin); m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMax[0],aabbMax); - node.m_escapeIndexOrTriangleIndex = triangleIndex; + node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; m_triangleNodes.push_back(node); } @@ -256,7 +259,7 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f btAssert(m_useQuantization); - int nodeSubPart=0; + int curNodeSubPart=-1; //get access info to trianglemesh data const unsigned char *vertexbase; @@ -267,7 +270,6 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f int indexstride; int numfaces; PHY_ScalarType indicestype; - meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart); btVector3 triangleVerts[3]; btVector3 aabbMin,aabbMax; @@ -282,7 +284,15 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f if (curNode.isLeafNode()) { //recalc aabb from triangle data + int nodeSubPart = curNode.getPartId(); int nodeTriangleIndex = curNode.getTriangleIndex(); + if (nodeSubPart != curNodeSubPart) + { + if (curNodeSubPart >= 0) + meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart); + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); + } //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); @@ -291,7 +301,7 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f for (int j=2;j>=0;j--) { - int graphicsindex = gfxbase[j]; + int graphicsindex = indicestype==PHY_SHORT?((short*)gfxbase)[j]:gfxbase[j]; btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); #ifdef DEBUG_PATCH_COLORS btVector3 mycolor = color[index&3]; @@ -347,7 +357,8 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f } - meshInterface->unLockReadOnlyVertexBase(nodeSubPart); + if (curNodeSubPart >= 0) + meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); } @@ -713,7 +724,7 @@ void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantize { if (isLeafNode) { - nodeCallback->processNode(0,currentNode->getTriangleIndex()); + nodeCallback->processNode(currentNode->getPartId(),currentNode->getTriangleIndex()); } else { //process left and right children @@ -730,7 +741,124 @@ void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantize +void btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const +{ + btAssert(m_useQuantization); + + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned boxBoxOverlap = 0; + unsigned rayBoxOverlap = 0; + + btScalar lambda_max = 1.0; +#define RAYAABB2 +#ifdef RAYAABB2 + btVector3 rayFrom = raySource; + btVector3 rayDirection = (rayTarget-raySource); + rayDirection.normalize (); + lambda_max = rayDirection.dot(rayTarget-raySource); + rayDirection[0] = btScalar(1.0) / rayDirection[0]; + rayDirection[1] = btScalar(1.0) / rayDirection[1]; + rayDirection[2] = btScalar(1.0) / rayDirection[2]; + unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; +#endif + + /* Quick pruning by quantized box */ + btVector3 rayAabbMin = raySource; + btVector3 rayAabbMax = raySource; + rayAabbMin.setMin(rayTarget); + rayAabbMax.setMax(rayTarget); + + /* Add box cast extents to bounding box */ + rayAabbMin += aabbMin; + rayAabbMax += aabbMax; + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin); + quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax); + + while (curIndex < endNodeIndex) + { + +//#define VISUALLY_ANALYZE_BVH 1 +#ifdef VISUALLY_ANALYZE_BVH + //some code snippet to debugDraw aabb, to visually analyze bvh structure + static int drawPatch = 0; + //need some global access to a debugDrawer + extern btIDebugDraw* debugDrawerPtr; + if (curIndex==drawPatch) + { + btVector3 aabbMin,aabbMax; + aabbMin = unQuantize(rootNode->m_quantizedAabbMin); + aabbMax = unQuantize(rootNode->m_quantizedAabbMax); + btVector3 color(1,0,0); + debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + } +#endif//VISUALLY_ANALYZE_BVH + + //catch bugs in tree data + assert (walkIterations < subTreeSize); + + walkIterations++; + //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); + isLeafNode = rootNode->isLeafNode(); + if (boxBoxOverlap) + { + btVector3 bounds[2]; + bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); + bounds[1] = unQuantize(rootNode->m_quantizedAabbMax); + /* Add box cast extents */ + bounds[0] += aabbMin; + bounds[1] += aabbMax; + btVector3 normal; +#if 0 + bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); + bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal); + if (ra2 != ra) + { + printf("functions don't match\n"); + } +#endif +#ifdef RAYAABB2 + rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); +#else + rayBoxOverlap = btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); +#endif + } + + if (isLeafNode && rayBoxOverlap) + { + nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); + } + + //PCK: unsigned instead of bool + if ((rayBoxOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} void btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const { @@ -776,7 +904,7 @@ void btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb if (isLeafNode && aabbOverlap) { - nodeCallback->processNode(0,rootNode->getTriangleIndex()); + nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); } //PCK: unsigned instead of bool @@ -820,15 +948,40 @@ void btOptimizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallba } - - -void btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btOptimizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const { - (void)nodeCallback; - (void)aabbMin; - (void)aabbMax; - //not yet, please use aabb - btAssert(0); + bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; + if (fast_path) + { + walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex); + } else { + /* Otherwise fallback to AABB overlap test */ + btVector3 aabbMin = raySource; + btVector3 aabbMax = raySource; + aabbMin.setMin(rayTarget); + aabbMax.setMax(rayTarget); + reportAabbOverlappingNodex(nodeCallback,aabbMin,aabbMax); + } +} + + +void btOptimizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const +{ + bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; + if (fast_path) + { + walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); + } else { + /* Slow path: + Construct the bounding box for the entire box cast and send that down the tree */ + btVector3 qaabbMin = raySource; + btVector3 qaabbMax = raySource; + qaabbMin.setMin(rayTarget); + qaabbMax.setMax(rayTarget); + qaabbMin += aabbMin; + qaabbMax += aabbMax; + reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); + } } @@ -1064,7 +1217,7 @@ bool btOptimizedBvh::serialize(void *o_alignedDataBuffer, unsigned i_dataBufferS return true; } -btOptimizedBvh *btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) +btOptimizedBvh *btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) { if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) @@ -1085,7 +1238,7 @@ btOptimizedBvh *btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un bvh->m_subtreeHeaderCount = btSwapEndian(bvh->m_subtreeHeaderCount); } - int calculatedBufSize = bvh->calculateSerializeBufferSize(); + unsigned int calculatedBufSize = bvh->calculateSerializeBufferSize(); btAssert(calculatedBufSize <= i_dataBufferSize); if (calculatedBufSize > i_dataBufferSize) @@ -1176,6 +1329,8 @@ m_bvhAabbMin(self.m_bvhAabbMin), m_bvhAabbMax(self.m_bvhAabbMax), m_bvhQuantization(self.m_bvhQuantization) { - + + } + diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/src/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index bcacdbe58..9cffe481d 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/src/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -31,6 +31,9 @@ class btStridingMeshInterface; //Note: currently we have 16 bytes per quantized node #define MAX_SUBTREE_SIZE_IN_BYTES 2048 +// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one +// actually) triangles each (since the sign bit is reserved +#define MAX_NUM_PARTS_IN_BITS 10 ///btQuantizedBvhNode is a compressed aabb node, 16 bytes. ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). @@ -57,7 +60,14 @@ ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode int getTriangleIndex() const { btAssert(isLeafNode()); - return m_escapeIndexOrTriangleIndex; + // Get only the lower bits where the triangle index is stored + return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); + } + int getPartId() const + { + btAssert(isLeafNode()); + // Get only the highest bits where the part index is stored + return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); } } ; @@ -276,6 +286,7 @@ protected: void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; ///tree traversal designed for small-memory processors like PS3 SPU @@ -319,8 +330,8 @@ public: void build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax); void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - void reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const; + void reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const; SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point) const { @@ -370,7 +381,7 @@ public: bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian); ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' - static btOptimizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian); + static btOptimizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); static unsigned int getAlignmentSerializationPadding(); @@ -391,3 +402,4 @@ private: #endif //OPTIMIZED_BVH_H + diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/src/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 30323deb3..4c9dd08fc 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/src/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#include +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" btPolyhedralConvexShape::btPolyhedralConvexShape() :m_localAabbMin(1,1,1), diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/src/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index 0cbce3abd..42b8cf113 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/src/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -51,6 +51,15 @@ public: virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; + const btVector3& getPlaneNormal() const + { + return m_planeNormal; + } + + const btScalar& getPlaneConstant() const + { + return m_planeConstant; + } //debugging virtual const char* getName()const {return "STATICPLANE";} diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/src/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index 554915a70..f4f938161 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/src/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -54,7 +54,7 @@ void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertex (*indexbase) = (unsigned char *)mesh.m_triangleIndexBase; indexstride = mesh.m_triangleIndexStride; - indicestype = PHY_INTEGER; + indicestype = mesh.m_indexType; } void btTriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const @@ -73,6 +73,6 @@ void btTriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned numfaces = mesh.m_numTriangles; (*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase; indexstride = mesh.m_triangleIndexStride; - indicestype = PHY_INTEGER; + indicestype = mesh.m_indexType; } diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/src/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index 3441a8325..9d8ec50de 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/src/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -34,7 +34,10 @@ ATTRIBUTE_ALIGNED16( struct) btIndexedMesh int m_numVertices; const unsigned char * m_vertexBase; int m_vertexStride; - int pad[2]; + // The index type is set when adding an indexed mesh to the + // btTriangleIndexVertexArray, do not set it manually + PHY_ScalarType m_indexType; + int pad; } ; @@ -62,11 +65,12 @@ public: virtual ~btTriangleIndexVertexArray(); //just to be backwards compatible - btTriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride); + btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride); - void addIndexedMesh(const btIndexedMesh& mesh) + void addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER) { m_indexedMeshes.push_back(mesh); + m_indexedMeshes[m_indexedMeshes.size()-1].m_indexType = indexType; } diff --git a/src/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/src/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index 6657fc091..c72ce19b9 100644 --- a/src/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/src/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -46,11 +46,6 @@ public: void recalcLocalAabb(); - virtual int getShapeType() const - { - return TRIANGLE_MESH_SHAPE_PROXYTYPE; - } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; diff --git a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index c6a2b396d..562c0dc47 100644 --- a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -174,6 +174,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( result.m_fraction = lastLambda; n = pointCollector.m_normalOnBInWorld; result.m_normal=n;//.setValue(1,1,1);// = n; + result.m_hitPoint = pointCollector.m_pointInWorld; return true; } c = pointCollector.m_pointInWorld; @@ -189,6 +190,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( result.m_fraction = lambda; result.m_normal = n; + result.m_hitPoint = c; return true; } diff --git a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index cc80f0aa8..ae35d5236 100644 --- a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -50,6 +50,7 @@ public: virtual ~CastResult() {}; btVector3 m_normal; + btVector3 m_hitPoint; btScalar m_fraction; btTransform m_hitTransformA; btTransform m_hitTransformB; diff --git a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp index da2a02b98..c9e690ab7 100644 --- a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -148,6 +148,7 @@ bool btGjkConvexCast::calcTimeOfImpact( //degeneracy, report a hit result.m_fraction = lastLambda; result.m_normal = n; + result.m_hitPoint = pointCollector.m_pointInWorld; return true; } c = pointCollector.m_pointInWorld; @@ -165,6 +166,7 @@ bool btGjkConvexCast::calcTimeOfImpact( result.m_fraction = lastLambda; result.m_normal = n; + result.m_hitPoint = c; return true; } } diff --git a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp index 14f2c0e07..e7fc970be 100644 --- a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp +++ b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp @@ -26,7 +26,7 @@ Nov.2006 #include "btGjkEpa.h" #include //for memset -#include +#include "LinearMath/btStackAlloc.h" #if defined(DEBUG) || defined (_DEBUG) #include //for debug printf diff --git a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index f0b1ce58d..763578bd3 100644 --- a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -70,6 +70,7 @@ public: btPersistentManifold(void* body0,void* body1,int bla) : m_body0(body0),m_body1(body1),m_cachedPoints(0) { + (void)bla; } SIMD_FORCE_INLINE void* getBody0() { return m_body0;} diff --git a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index 31b914677..4f4391ff4 100644 --- a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -13,7 +13,14 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +#include +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" #include "btRaycastCallback.h" btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to) @@ -29,8 +36,6 @@ btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) { - - const btVector3 &vert0=triangle[0]; const btVector3 &vert1=triangle[1]; const btVector3 &vert2=triangle[2]; @@ -99,3 +104,55 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, } } } + + +btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld) +{ + m_convexShape = convexShape; + m_convexShapeFrom = convexShapeFrom; + m_convexShapeTo = convexShapeTo; + m_triangleToWorld = triangleToWorld; + m_hitFraction = 1.0; +} + +void +btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex) +{ + btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]); + + btVoronoiSimplexSolver simplexSolver; + + +//#define USE_SUBSIMPLEX_CONVEX_CAST 1 +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver); +#else + //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver); + btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,NULL); +#endif //#USE_SUBSIMPLEX_CONVEX_CAST + + btConvexCast::CastResult castResult; + castResult.m_fraction = btScalar(1.); + if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < m_hitFraction) + { + +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + //rotate normal into worldspace + castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal; +#endif //USE_SUBSIMPLEX_CONVEX_CAST + castResult.m_normal.normalize(); + + reportHit (castResult.m_normal, + castResult.m_hitPoint, + castResult.m_fraction, + partId, + triangleIndex); + } + } + } +} \ No newline at end of file diff --git a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index 87490a345..0d5eaf8eb 100644 --- a/src/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/src/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -17,14 +17,15 @@ subject to the following restrictions: #define RAYCAST_TRI_CALLBACK_H #include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "LinearMath/btTransform.h" struct btBroadphaseProxy; - +class btConvexShape; class btTriangleRaycastCallback: public btTriangleCallback { public: - //input + //input btVector3 m_from; btVector3 m_to; @@ -38,5 +39,21 @@ public: }; +class btTriangleConvexcastCallback : public btTriangleCallback +{ +public: + const btConvexShape* m_convexShape; + btTransform m_convexShapeFrom; + btTransform m_convexShapeTo; + btTransform m_triangleToWorld; + btScalar m_hitFraction; + + btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld); + + virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex); + + virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; +}; + #endif //RAYCAST_TRI_CALLBACK_H diff --git a/src/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 8d8adfe92..01e6e7fa4 100644 --- a/src/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -109,6 +109,17 @@ public: } + btScalar getLowerLimit() const + { + return m_lowerLimit; + } + + btScalar getUpperLimit() const + { + return m_upperLimit; + } + + btScalar getHingeAngle(); diff --git a/src/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 7d4ed7856..91b252527 100644 --- a/src/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -33,11 +33,9 @@ subject to the following restrictions: #include "btSolverBody.h" #include "btSolverConstraint.h" + #include "LinearMath/btAlignedObjectArray.h" -#ifdef USE_PROFILE -#include "LinearMath/btQuickprof.h" -#endif //USE_PROFILE int totalCpd = 0; @@ -129,23 +127,28 @@ btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() } -void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody) +void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) { -/* int size = sizeof(btSolverBody); - int sizeofrb = sizeof(btRigidBody); - int sizemanifold = sizeof(btPersistentManifold); - int sizeofmp = sizeof(btManifoldPoint); - int sizeofPersistData = sizeof (btConstraintPersistentData); -*/ - - solverBody->m_angularVelocity = rigidbody->getAngularVelocity(); - solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition(); - solverBody->m_friction = rigidbody->getFriction(); -// solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld(); - solverBody->m_invMass = rigidbody->getInvMass(); - solverBody->m_linearVelocity = rigidbody->getLinearVelocity(); - solverBody->m_originalBody = rigidbody; - solverBody->m_angularFactor = rigidbody->getAngularFactor(); + btRigidBody* rb = btRigidBody::upcast(collisionObject); + if (rb) + { + solverBody->m_angularVelocity = rb->getAngularVelocity() ; + solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin(); + solverBody->m_friction = collisionObject->getFriction(); + solverBody->m_invMass = rb->getInvMass(); + solverBody->m_linearVelocity = rb->getLinearVelocity(); + solverBody->m_originalBody = rb; + solverBody->m_angularFactor = rb->getAngularFactor(); + } else + { + solverBody->m_angularVelocity.setValue(0,0,0); + solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin(); + solverBody->m_friction = collisionObject->getFriction(); + solverBody->m_invMass = 0.f; + solverBody->m_linearVelocity.setValue(0,0,0); + solverBody->m_originalBody = 0; + solverBody->m_angularFactor = 1.f; + } } btScalar penetrationResolveFactor = btScalar(0.9); @@ -368,9 +371,12 @@ btScalar resolveSingleFrictionCacheFriendly( -void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btRigidBody* rb0,btRigidBody* rb1, btScalar relaxation) +void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation) { + btRigidBody* body0=btRigidBody::upcast(colObj0); + btRigidBody* body1=btRigidBody::upcast(colObj1); + btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand(); solverConstraint.m_contactNormal = normalAxis; @@ -387,12 +393,12 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& { btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1; + solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0); } { btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1; + solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0); } #ifdef COMPUTE_IMPULSE_DENOM @@ -400,21 +406,32 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); #else btVector3 vec; - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - btScalar denom0 = rb0->getInvMass() + normalAxis.dot(vec); - vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); - btScalar denom1 = rb1->getInvMass() + normalAxis.dot(vec); + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + if (body0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = body0->getInvMass() + normalAxis.dot(vec); + } + if (body1) + { + vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = body1->getInvMass() + normalAxis.dot(vec); + } #endif //COMPUTE_IMPULSE_DENOM btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; + } + btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { + BT_PROFILE("solveGroupCacheFriendlySetup"); (void)stackAlloc; (void)debugDrawer; @@ -424,7 +441,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol return 0.f; } btPersistentManifold* manifold = 0; - btRigidBody* rb0=0,*rb1=0; + btCollisionObject* colObj0=0,*colObj1=0; + + //btRigidBody* rb0=0,*rb1=0; + #ifdef FORCE_REFESH_CONTACT_MANIFOLDS @@ -450,7 +470,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btVector3 color(0,1,0); - BEGIN_PROFILE("gatherSolverData"); //int sizeofSB = sizeof(btSolverBody); //int sizeofSC = sizeof(btSolverConstraint); @@ -498,10 +517,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol for (i=0;igetBody1(); - - rb0 = (btRigidBody*)manifold->getBody0(); - + colObj0 = (btCollisionObject*)manifold->getBody0(); + colObj1 = (btCollisionObject*)manifold->getBody1(); int solverBodyIdA=-1; int solverBodyIdB=-1; @@ -511,45 +528,45 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol - if (rb0->getIslandTag() >= 0) + if (colObj0->getIslandTag() >= 0) { - if (rb0->getCompanionId() >= 0) + if (colObj0->getCompanionId() >= 0) { //body has already been converted - solverBodyIdA = rb0->getCompanionId(); + solverBodyIdA = colObj0->getCompanionId(); } else { solverBodyIdA = m_tmpSolverBodyPool.size(); btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb0); - rb0->setCompanionId(solverBodyIdA); + initSolverBody(&solverBody,colObj0); + colObj0->setCompanionId(solverBodyIdA); } } else { //create a static body solverBodyIdA = m_tmpSolverBodyPool.size(); btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb0); + initSolverBody(&solverBody,colObj0); } - if (rb1->getIslandTag() >= 0) + if (colObj1->getIslandTag() >= 0) { - if (rb1->getCompanionId() >= 0) + if (colObj1->getCompanionId() >= 0) { - solverBodyIdB = rb1->getCompanionId(); + solverBodyIdB = colObj1->getCompanionId(); } else { solverBodyIdB = m_tmpSolverBodyPool.size(); btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb1); - rb1->setCompanionId(solverBodyIdB); + initSolverBody(&solverBody,colObj1); + colObj1->setCompanionId(solverBodyIdB); } } else { //create a static body solverBodyIdB = m_tmpSolverBodyPool.size(); btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb1); + initSolverBody(&solverBody,colObj1); } } @@ -572,8 +589,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); - rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); - rel_pos2 = pos2 - rb1->getCenterOfMassPosition(); + rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); relaxation = 1.f; @@ -584,25 +601,35 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol { btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand(); + btRigidBody* rb0 = btRigidBody::upcast(colObj0); + btRigidBody* rb1 = btRigidBody::upcast(colObj1); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0; + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0); btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1; + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0); { #ifdef COMPUTE_IMPULSE_DENOM btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); #else btVector3 vec; - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - btScalar denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); - vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); - btScalar denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); + } + if (rb1) + { + vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); + } #endif //COMPUTE_IMPULSE_DENOM btScalar denom = relaxation/(denom0+denom1); @@ -614,8 +641,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB); - btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2); + btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); + btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); vel = vel1 - vel2; @@ -647,36 +674,35 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } - + { btVector3 frictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = frictionDir1.length2(); if (lat_rel_vel > SIMD_EPSILON)//0.0f) { frictionDir1 /= btSqrt(lat_rel_vel); - addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation); + addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); btVector3 frictionDir2 = frictionDir1.cross(cp.m_normalWorldOnB); frictionDir2.normalize();//?? - addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation); + addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } else { //re-calculate friction direction every frame, todo: check if this is really needed btVector3 frictionDir1,frictionDir2; btPlaneSpace1(cp.m_normalWorldOnB,frictionDir1,frictionDir2); - addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation); - addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation); + addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } } + + } } } } } - END_PROFILE("gatherSolverData"); - - BEGIN_PROFILE("prepareConstraints"); - + btContactSolverInfo info = infoGlobal; { @@ -710,15 +736,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol - - END_PROFILE("prepareConstraints"); return 0.f; } btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { - BEGIN_PROFILE("solveConstraintsIterations"); + BT_PROFILE("solveGroupCacheFriendlyIterations"); int numConstraintPool = m_tmpSolverConstraintPool.size(); int numFrictionPool = m_tmpSolverFrictionConstraintPool.size(); @@ -750,6 +774,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( for (j=0;jcalculateLocalInertia(mass,localInertia); } void* mem = btAlignedAlloc(sizeof(btRigidBody),16); - btRigidBody* body = new (mem)btRigidBody(mass, 0,shape,localInertia); + btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia); + btRigidBody* body = new (mem)btRigidBody(rbci); body->setWorldTransform(trans); body->setUserPointer(user_data); return (plRigidBodyHandle) body; diff --git a/src/bullet/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp b/src/bullet/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp index 5e330cb64..451ec1f54 100644 --- a/src/bullet/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp +++ b/src/bullet/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp @@ -47,6 +47,7 @@ void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) { startProfiling(timeStep); + ///update aabbs information updateAabbs(); @@ -115,14 +116,11 @@ void btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep) ///calculate time of impact for overlapping pairs - BEGIN_PROFILE("performContinuousCollisionDetection"); btDispatcher* dispatcher = getDispatcher(); if (dispatcher) dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); - END_PROFILE("performContinuousCollisionDetection"); - toi = dispatchInfo.m_timeOfImpact; dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE; @@ -131,7 +129,6 @@ void btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep) void btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep) { - BEGIN_PROFILE("perform Temporal Broadphase Collision Detection"); btVector3 temporalAabbMin,temporalAabbMax; @@ -186,7 +183,6 @@ void btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep) m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); - END_PROFILE("perform Temporal Broadphase Collision Detection"); } diff --git a/src/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index d047474c8..ba89e8c23 100644 --- a/src/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -21,7 +21,8 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btQuickprof.h" //rigidbody & constraints #include "BulletDynamics/Dynamics/btRigidBody.h" @@ -60,7 +61,6 @@ subject to the following restrictions: btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) :btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), m_constraintSolver(constraintSolver), -m_debugDrawer(0), m_gravity(0,-10,0), m_localTime(btScalar(1.)/btScalar(60.)), m_profileTimings(0) @@ -122,14 +122,14 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) } } -void btDiscreteDynamicsWorld::synchronizeMotionStates() +void btDiscreteDynamicsWorld::debugDrawWorld() { - //debug vehicle wheels - - + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) { + int i; + //todo: iterate over awake simulation islands! - for ( int i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) @@ -155,6 +155,88 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); } + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btPoint3 minAabb,maxAabb; + btVector3 colorvec(1,0,0); + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); + } + + } + + for ( i=0;im_vehicles.size();i++) + { + for (int v=0;vgetNumWheels();v++) + { + btVector3 wheelColor(0,255,255); + if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact) + { + wheelColor.setValue(0,0,255); + } else + { + wheelColor.setValue(255,0,255); + } + + btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin(); + + btVector3 axle = btVector3( + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()], + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()], + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]); + + + //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS + //debug wheels (cylinders) + m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); + m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); + + } + } + } +} + +void btDiscreteDynamicsWorld::clearForces() +{ + //todo: iterate over awake simulation islands! + for ( int i=0;iclearForces(); + } + } +} + +///apply gravity, call this once per timestep +void btDiscreteDynamicsWorld::applyGravity() +{ + //todo: iterate over awake simulation islands! + for ( int i=0;iisActive()) + { + body->applyGravity(); + } + } +} + + + +void btDiscreteDynamicsWorld::synchronizeMotionStates() +{ + { + //todo: iterate over awake simulation islands! + for ( int i=0;igetMotionState() && !body->isStaticOrKinematicObject()) { @@ -178,31 +260,8 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() { for (int v=0;vgetNumWheels();v++) { - btVector3 wheelColor(0,255,255); - if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact) - { - wheelColor.setValue(0,0,255); - } else - { - wheelColor.setValue(255,0,255); - } - //synchronize the wheels with the (interpolated) chassis worldtransform m_vehicles[i]->updateWheelTransform(v,true); - - btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin(); - - btVector3 axle = btVector3( - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()], - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()], - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]); - - - //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS - //debug wheels (cylinders) - m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); - m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); - } } } @@ -212,6 +271,10 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) { + startProfiling(timeStep); + + BT_PROFILE("stepSimulation"); + int numSimulationSubSteps = 0; if (maxSubSteps) @@ -249,6 +312,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, saveKinematicState(fixedTimeStep); + applyGravity(); + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; @@ -262,16 +327,17 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, synchronizeMotionStates(); + clearForces(); + + CProfileManager::Increment_Frame_Counter(); + return numSimulationSubSteps; } void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - startProfiling(timeStep); - - ///update aabbs information - updateAabbs(); + BT_PROFILE("internalSingleStepSimulation"); ///apply gravity, predict motion predictUnconstraintMotion(timeStep); @@ -306,8 +372,6 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) updateActivationState( timeStep ); - - } void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) @@ -363,19 +427,18 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) { - BEGIN_PROFILE("updateVehicles"); - + BT_PROFILE("updateVehicles"); + for ( int i=0;iupdateVehicle( timeStep); } - END_PROFILE("updateVehicles"); } void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { - BEGIN_PROFILE("updateActivationState"); + BT_PROFILE("updateActivationState"); for ( int i=0;iupdateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); @@ -599,66 +662,14 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() //Store the island id in each body getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - END_PROFILE("calculateSimulationIslands"); - + } -void btDiscreteDynamicsWorld::updateAabbs() -{ - BEGIN_PROFILE("updateAabbs"); - - btVector3 colorvec(1,0,0); - btTransform predictedTrans; - for ( int i=0;iIsActive() && (!body->IsStatic())) - { - btPoint3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; - - //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) - { - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } else - { - //something went wrong, investigate - //this assert is unwanted in 3D modelers (danger of loosing work) - body->setActivationState(DISABLE_SIMULATION); - - static bool reportMe = true; - if (reportMe && m_debugDrawer) - { - reportMe = false; - m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); - m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); - m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); - m_debugDrawer->reportErrorWarning("Thanks.\n"); - } - - - } - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); - } - } - } - } - - END_PROFILE("updateAabbs"); -} void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { - BEGIN_PROFILE("integrateTransforms"); + BT_PROFILE("integrateTransforms"); btTransform predictedTrans; for ( int i=0;iisActive()) { - body->applyForces( timeStep); body->integrateVelocities( timeStep); + //damping + body->applyDamping(timeStep); + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); } } } } - END_PROFILE("predictUnconstraintMotion"); } void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) { (void)timeStep; + + CProfileManager::Reset(); + + #ifdef USE_QUICKPROF diff --git a/src/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 7364c4cd6..9d21c756a 100644 --- a/src/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -1,164 +1,161 @@ -/* -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. -*/ - -#ifndef BT_DISCRETE_DYNAMICS_WORLD_H -#define BT_DISCRETE_DYNAMICS_WORLD_H - -#include "btDynamicsWorld.h" - -class btDispatcher; -class btOverlappingPairCache; -class btConstraintSolver; -class btSimulationIslandManager; -class btTypedConstraint; -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -class btRaycastVehicle; -class btIDebugDraw; -#include "LinearMath/btAlignedObjectArray.h" - - -///btDiscreteDynamicsWorld provides discrete rigid body simulation -///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController -class btDiscreteDynamicsWorld : public btDynamicsWorld -{ -protected: - - btConstraintSolver* m_constraintSolver; - - btSimulationIslandManager* m_islandManager; - - btAlignedObjectArray m_constraints; - - btIDebugDraw* m_debugDrawer; - - btVector3 m_gravity; - - //for variable timesteps - btScalar m_localTime; - //for variable timesteps - - bool m_ownsIslandManager; - bool m_ownsConstraintSolver; - - btContactSolverInfo m_solverInfo; - - - btAlignedObjectArray m_vehicles; - - int m_profileTimings; - - void predictUnconstraintMotion(btScalar timeStep); - - void integrateTransforms(btScalar timeStep); - - void calculateSimulationIslands(); - - void solveConstraints(btContactSolverInfo& solverInfo); - - void updateActivationState(btScalar timeStep); - - void updateVehicles(btScalar timeStep); - - void startProfiling(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); - - void synchronizeMotionStates(); - - void saveKinematicState(btScalar timeStep); - - void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); - -public: - - - ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those - btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - - virtual ~btDiscreteDynamicsWorld(); - - ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); - - virtual void updateAabbs(); - - void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); - - void removeConstraint(btTypedConstraint* constraint); - - void addVehicle(btRaycastVehicle* vehicle); - - void removeVehicle(btRaycastVehicle* vehicle); - - btSimulationIslandManager* getSimulationIslandManager() - { - return m_islandManager; - } - - const btSimulationIslandManager* getSimulationIslandManager() const - { - return m_islandManager; - } - - btCollisionWorld* getCollisionWorld() - { - return this; - } - - virtual void setDebugDrawer(btIDebugDraw* debugDrawer) - { - m_debugDrawer = debugDrawer; - } - - virtual btIDebugDraw* getDebugDrawer() - { - return m_debugDrawer; - } - - virtual void setGravity(const btVector3& gravity); - - virtual void addRigidBody(btRigidBody* body); - - virtual void addRigidBody(btRigidBody* body, short group, short mask); - - virtual void removeRigidBody(btRigidBody* body); - - void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); - - virtual void setConstraintSolver(btConstraintSolver* solver); - - virtual btConstraintSolver* getConstraintSolver(); - - virtual int getNumConstraints() const; - - virtual btTypedConstraint* getConstraint(int index) ; - - virtual const btTypedConstraint* getConstraint(int index) const; - - btContactSolverInfo& getSolverInfo() - { - return m_solverInfo; - } - - virtual btDynamicsWorldType getWorldType() const - { - return BT_DISCRETE_DYNAMICS_WORLD; - } - - -}; - -#endif //BT_DISCRETE_DYNAMICS_WORLD_H +/* +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. +*/ + +#ifndef BT_DISCRETE_DYNAMICS_WORLD_H +#define BT_DISCRETE_DYNAMICS_WORLD_H + +#include "btDynamicsWorld.h" + +class btDispatcher; +class btOverlappingPairCache; +class btConstraintSolver; +class btSimulationIslandManager; +class btTypedConstraint; +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +class btRaycastVehicle; +class btIDebugDraw; +#include "LinearMath/btAlignedObjectArray.h" + + +///btDiscreteDynamicsWorld provides discrete rigid body simulation +///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController +class btDiscreteDynamicsWorld : public btDynamicsWorld +{ +protected: + + btConstraintSolver* m_constraintSolver; + + btSimulationIslandManager* m_islandManager; + + btAlignedObjectArray m_constraints; + + + btVector3 m_gravity; + + //for variable timesteps + btScalar m_localTime; + //for variable timesteps + + bool m_ownsIslandManager; + bool m_ownsConstraintSolver; + + btContactSolverInfo m_solverInfo; + + + btAlignedObjectArray m_vehicles; + + int m_profileTimings; + + virtual void predictUnconstraintMotion(btScalar timeStep); + + void integrateTransforms(btScalar timeStep); + + void calculateSimulationIslands(); + + void solveConstraints(btContactSolverInfo& solverInfo); + + void updateActivationState(btScalar timeStep); + + void updateVehicles(btScalar timeStep); + + void startProfiling(btScalar timeStep); + + virtual void internalSingleStepSimulation( btScalar timeStep); + + void synchronizeMotionStates(); + + void saveKinematicState(btScalar timeStep); + + void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); + +public: + + + ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those + btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + + virtual ~btDiscreteDynamicsWorld(); + + ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); + + + void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); + + void removeConstraint(btTypedConstraint* constraint); + + void addVehicle(btRaycastVehicle* vehicle); + + void removeVehicle(btRaycastVehicle* vehicle); + + btSimulationIslandManager* getSimulationIslandManager() + { + return m_islandManager; + } + + const btSimulationIslandManager* getSimulationIslandManager() const + { + return m_islandManager; + } + + btCollisionWorld* getCollisionWorld() + { + return this; + } + + + virtual void setGravity(const btVector3& gravity); + + virtual void addRigidBody(btRigidBody* body); + + virtual void addRigidBody(btRigidBody* body, short group, short mask); + + virtual void removeRigidBody(btRigidBody* body); + + void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + + virtual void debugDrawWorld(); + + virtual void setConstraintSolver(btConstraintSolver* solver); + + virtual btConstraintSolver* getConstraintSolver(); + + virtual int getNumConstraints() const; + + virtual btTypedConstraint* getConstraint(int index) ; + + virtual const btTypedConstraint* getConstraint(int index) const; + + btContactSolverInfo& getSolverInfo() + { + return m_solverInfo; + } + + virtual btDynamicsWorldType getWorldType() const + { + return BT_DISCRETE_DYNAMICS_WORLD; + } + + ///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep. + virtual void clearForces(); + + ///apply gravity, call this once per timestep + virtual void applyGravity(); + + +}; + +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/src/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h index aaf734a09..974b6c725 100644 --- a/src/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/src/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -1,89 +1,87 @@ -/* -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. -*/ - -#ifndef BT_DYNAMICS_WORLD_H -#define BT_DYNAMICS_WORLD_H - -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -class btTypedConstraint; -class btRaycastVehicle; -class btConstraintSolver; - - +/* +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. +*/ + +#ifndef BT_DYNAMICS_WORLD_H +#define BT_DYNAMICS_WORLD_H + +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +class btTypedConstraint; +class btRaycastVehicle; +class btConstraintSolver; + + enum btDynamicsWorldType { BT_SIMPLE_DYNAMICS_WORLD=1, BT_DISCRETE_DYNAMICS_WORLD=2, BT_CONTINUOUS_DYNAMICS_WORLD=3 -}; - -///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous -class btDynamicsWorld : public btCollisionWorld -{ - public: - - - btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) - :btCollisionWorld(dispatcher,broadphase,collisionConfiguration) - { - } - - virtual ~btDynamicsWorld() - { - } - - ///stepSimulation proceeds the simulation over timeStep units - ///if maxSubSteps > 0, it will interpolate time steps - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; - - virtual void updateAabbs() = 0; - - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) { (void)constraint;}; - - virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}; - - virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}; - - virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}; - - - virtual void setDebugDrawer(btIDebugDraw* debugDrawer) = 0; - - virtual btIDebugDraw* getDebugDrawer() = 0; - - //once a rigidbody is added to the dynamics world, it will get this gravity assigned - //existing rigidbodies in the world get gravity assigned too, during this method - virtual void setGravity(const btVector3& gravity) = 0; - - virtual void addRigidBody(btRigidBody* body) = 0; - - virtual void removeRigidBody(btRigidBody* body) = 0; - - virtual void setConstraintSolver(btConstraintSolver* solver) = 0; - - virtual btConstraintSolver* getConstraintSolver() = 0; - - virtual int getNumConstraints() const { return 0; } - - virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } - - virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } - - virtual btDynamicsWorldType getWorldType() const=0; - -}; - -#endif //BT_DYNAMICS_WORLD_H - +}; + +///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous +class btDynamicsWorld : public btCollisionWorld +{ + public: + + + btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) + :btCollisionWorld(dispatcher,broadphase,collisionConfiguration) + { + } + + virtual ~btDynamicsWorld() + { + } + + ///stepSimulation proceeds the simulation over timeStep units + ///if maxSubSteps > 0, it will interpolate time steps + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; + + virtual void debugDrawWorld() = 0; + + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) { (void)constraint;}; + + virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}; + + virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}; + + virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}; + + //once a rigidbody is added to the dynamics world, it will get this gravity assigned + //existing rigidbodies in the world get gravity assigned too, during this method + virtual void setGravity(const btVector3& gravity) = 0; + + virtual void addRigidBody(btRigidBody* body) = 0; + + virtual void removeRigidBody(btRigidBody* body) = 0; + + virtual void setConstraintSolver(btConstraintSolver* solver) = 0; + + virtual btConstraintSolver* getConstraintSolver() = 0; + + virtual int getNumConstraints() const { return 0; } + + virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } + + virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } + + virtual btDynamicsWorldType getWorldType() const=0; + + virtual void clearForces() = 0; + + +}; + +#endif //BT_DYNAMICS_WORLD_H + diff --git a/src/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp index 03e60acdb..690662366 100644 --- a/src/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -20,38 +20,51 @@ subject to the following restrictions: #include "LinearMath/btMotionState.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -btScalar gLinearAirDamping = btScalar(1.); //'temporarily' global variables btScalar gDeactivationTime = btScalar(2.); bool gDisableDeactivation = false; - -btScalar gLinearSleepingThreshold = btScalar(0.8); -btScalar gAngularSleepingThreshold = btScalar(1.0); static int uniqueId = 0; -btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) -: - m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)), - m_angularFactor(btScalar(1.)), - m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_linearDamping(btScalar(0.)), - m_angularDamping(btScalar(0.5)), - m_linearSleepingThreshold(gLinearSleepingThreshold), - m_angularSleepingThreshold(gAngularSleepingThreshold), - m_optionalMotionState(motionState), - m_contactSolverType(0), - m_frictionSolverType(0) + +btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) +{ + setupRigidBody(constructionInfo); +} + +btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia) +{ + btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); + setupRigidBody(cinfo); +} + +void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) { - if (motionState) + m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_angularFactor = btScalar(1.); + m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_linearDamping = btScalar(0.); + m_angularDamping = btScalar(0.5); + m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; + m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; + m_optionalMotionState = constructionInfo.m_motionState; + m_contactSolverType = 0; + m_frictionSolverType = 0; + m_additionalDamping = constructionInfo.m_additionalDamping; + + m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr; + m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr; + m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor; + + if (m_optionalMotionState) { - motionState->getWorldTransform(m_worldTransform); + m_optionalMotionState->getWorldTransform(m_worldTransform); } else { - m_worldTransform = btTransform::getIdentity(); + m_worldTransform = constructionInfo.m_startWorldTransform; } m_interpolationWorldTransform = m_worldTransform; @@ -59,93 +72,24 @@ btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionS m_interpolationAngularVelocity.setValue(0,0,0); //moved to btCollisionObject - m_friction = friction; - m_restitution = restitution; + m_friction = constructionInfo.m_friction; + m_restitution = constructionInfo.m_restitution; - m_collisionShape = collisionShape; + m_collisionShape = constructionInfo.m_collisionShape; m_debugBodyId = uniqueId++; //m_internalOwner is to allow upcasting from collision object to rigid body m_internalOwner = this; - setMassProps(mass, localInertia); - setDamping(linearDamping, angularDamping); + setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); + setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); updateInertiaTensor(); } -#ifdef OBSOLETE_MOTIONSTATE_LESS -btRigidBody::btRigidBody( btScalar mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) -: - m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)), - m_linearSleepingThreshold(gLinearSleepingThreshold), - m_angularSleepingThreshold(gAngularSleepingThreshold), - m_linearDamping(btScalar(0.)), - m_angularDamping(btScalar(0.5)), - m_optionalMotionState(0), - m_contactSolverType(0), - m_frictionSolverType(0) - -{ - - m_worldTransform = worldTransform; - m_interpolationWorldTransform = m_worldTransform; - m_interpolationLinearVelocity.setValue(0,0,0); - m_interpolationAngularVelocity.setValue(0,0,0); - - //moved to btCollisionObject - m_friction = friction; - m_restitution = restitution; - - m_collisionShape = collisionShape; - m_debugBodyId = uniqueId++; - - //m_internalOwner is to allow upcasting from collision object to rigid body - m_internalOwner = this; - - setMassProps(mass, localInertia); - setDamping(linearDamping, angularDamping); - updateInertiaTensor(); - -} - -#endif //OBSOLETE_MOTIONSTATE_LESS - - - - -#define EXPERIMENTAL_JITTER_REMOVAL 1 -#ifdef EXPERIMENTAL_JITTER_REMOVAL -//Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate -//doesn't work very well yet (value 0 disabled this damping) -//note there this influences deactivation thresholds! -btScalar gClippedAngvelThresholdSqr = btScalar(0.01); -btScalar gClippedLinearThresholdSqr = btScalar(0.01); -#endif //EXPERIMENTAL_JITTER_REMOVAL - -btScalar gJitterVelocityDampingFactor = btScalar(0.7); void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) { - -#ifdef EXPERIMENTAL_JITTER_REMOVAL - //if (wantsSleeping()) - { - //clip to avoid jitter - if ((m_angularVelocity.length2() < gClippedAngvelThresholdSqr) && - (m_linearVelocity.length2() < gClippedLinearThresholdSqr)) - { - m_angularVelocity *= gJitterVelocityDampingFactor; - m_linearVelocity *= gJitterVelocityDampingFactor; - } - } - -#endif //EXPERIMENTAL_JITTER_REMOVAL - btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); } @@ -196,50 +140,63 @@ void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) -#include + +///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping +void btRigidBody::applyDamping(btScalar timeStep) +{ + m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); + m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); + + if (m_additionalDamping) + { + //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. + //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete + if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) && + (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr)) + { + m_angularVelocity *= m_additionalDampingFactor; + m_linearVelocity *= m_additionalDampingFactor; + } + + + btScalar speed = m_linearVelocity.length(); + if (speed < m_linearDamping) + { + btScalar dampVel = btScalar(0.005); + if (speed > dampVel) + { + btVector3 dir = m_linearVelocity.normalized(); + m_linearVelocity -= dir * dampVel; + } else + { + m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + } + } + + btScalar angSpeed = m_angularVelocity.length(); + if (angSpeed < m_angularDamping) + { + btScalar angDampVel = btScalar(0.005); + if (angSpeed > angDampVel) + { + btVector3 dir = m_angularVelocity.normalized(); + m_angularVelocity -= dir * angDampVel; + } else + { + m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + } + } + } +} -void btRigidBody::applyForces(btScalar step) +void btRigidBody::applyGravity() { if (isStaticOrKinematicObject()) return; applyCentralForce(m_gravity); - - m_linearVelocity *= GEN_clamped((btScalar(1.) - step * gLinearAirDamping * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); - m_angularVelocity *= GEN_clamped((btScalar(1.) - step * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); -#define FORCE_VELOCITY_DAMPING 1 -#ifdef FORCE_VELOCITY_DAMPING - btScalar speed = m_linearVelocity.length(); - if (speed < m_linearDamping) - { - btScalar dampVel = btScalar(0.005); - if (speed > dampVel) - { - btVector3 dir = m_linearVelocity.normalized(); - m_linearVelocity -= dir * dampVel; - } else - { - m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - } - } - - btScalar angSpeed = m_angularVelocity.length(); - if (angSpeed < m_angularDamping) - { - btScalar angDampVel = btScalar(0.005); - if (angSpeed > angDampVel) - { - btVector3 dir = m_angularVelocity.normalized(); - m_angularVelocity -= dir * angDampVel; - } else - { - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - } - } -#endif //FORCE_VELOCITY_DAMPING - } void btRigidBody::proceedToTransform(const btTransform& newTrans) @@ -290,7 +247,6 @@ void btRigidBody::integrateVelocities(btScalar step) m_angularVelocity *= (MAX_ANGVEL/step) /angvel; } - clearForces(); } btQuaternion btRigidBody::getOrientation() const diff --git a/src/bullet/src/BulletDynamics/Dynamics/btRigidBody.h b/src/bullet/src/BulletDynamics/Dynamics/btRigidBody.h index b11f9f76d..c9badae9f 100644 --- a/src/bullet/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/bullet/src/BulletDynamics/Dynamics/btRigidBody.h @@ -27,16 +27,18 @@ class btMotionState; class btTypedConstraint; -extern btScalar gLinearAirDamping; - extern btScalar gDeactivationTime; extern bool gDisableDeactivation; -extern btScalar gLinearSleepingThreshold; -extern btScalar gAngularSleepingThreshold; -/// btRigidBody class for btRigidBody Dynamics -/// +///btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. +///It is recommended for performance and memory use to share btCollisionShape objects whenever possible. +///There are 3 types of rigid bodies: +///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics. +///- B) Fixed objects with zero mass. They are not moving (basically collision objects) +///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform. +///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time. +///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects) class btRigidBody : public btCollisionObject { @@ -54,9 +56,15 @@ class btRigidBody : public btCollisionObject btScalar m_linearDamping; btScalar m_angularDamping; + bool m_additionalDamping; + btScalar m_additionalDampingFactor; + btScalar m_additionalLinearDampingThresholdSqr; + btScalar m_additionalAngularDampingThresholdSqr; + btScalar m_additionalAngularDampingFactor; + + btScalar m_linearSleepingThreshold; btScalar m_angularSleepingThreshold; - //m_optionalMotionState allows to automatic synchronize the world transform for active objects btMotionState* m_optionalMotionState; @@ -66,20 +74,85 @@ class btRigidBody : public btCollisionObject public: -#ifdef OBSOLETE_MOTIONSTATE_LESS - //not supported, please use btMotionState - btRigidBody(btScalar mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.)); -#endif //OBSOLETE_MOTIONSTATE_LESS - btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.)); + ///btRigidBodyConstructionInfo provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. + ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument) + ///You can use the motion state to synchronize the world transform between physics and graphics objects. + ///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state, + ///m_startWorldTransform is only used when you don't provide a motion state. + struct btRigidBodyConstructionInfo + { + btScalar m_mass; - virtual ~btRigidBody() + ///When a motionState is provided, the rigid body will initialize its world transform from the motion state + ///In this case, m_startWorldTransform is ignored. + btMotionState* m_motionState; + btTransform m_startWorldTransform; + + btCollisionShape* m_collisionShape; + btVector3 m_localInertia; + btScalar m_linearDamping; + btScalar m_angularDamping; + + ///best simulation results when friction is non-zero + btScalar m_friction; + ///best simulation results using zero restitution. + btScalar m_restitution; + + btScalar m_linearSleepingThreshold; + btScalar m_angularSleepingThreshold; + + //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. + //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete + bool m_additionalDamping; + btScalar m_additionalDampingFactor; + btScalar m_additionalLinearDampingThresholdSqr; + btScalar m_additionalAngularDampingThresholdSqr; + btScalar m_additionalAngularDampingFactor; + + + btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)): + m_mass(mass), + m_motionState(motionState), + m_collisionShape(collisionShape), + m_localInertia(localInertia), + m_linearDamping(btScalar(0.)), + m_angularDamping(btScalar(0.)), + m_friction(btScalar(0.5)), + m_restitution(btScalar(0.)), + m_linearSleepingThreshold(btScalar(0.8)), + m_angularSleepingThreshold(btScalar(1.f)), + m_additionalDamping(false), + m_additionalDampingFactor(btScalar(0.005)), + m_additionalLinearDampingThresholdSqr(btScalar(0.01)), + m_additionalAngularDampingThresholdSqr(btScalar(0.01)), + m_additionalAngularDampingFactor(btScalar(0.01)) + { + m_startWorldTransform.setIdentity(); + } + }; + + ///btRigidBody constructor using construction info + btRigidBody( const btRigidBodyConstructionInfo& constructionInfo); + + ///btRigidBody constructor for backwards compatibility. + ///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo) + btRigidBody( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)); + + + virtual ~btRigidBody() { //No constraints should point to this rigidbody //Remove constraints from the dynamics world before you delete the related rigidbodies. btAssert(m_constraintRefs.size()==0); } +protected: + + ///setupRigidBody is only used internally by the constructor + void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); + +public: void proceedToTransform(const btTransform& newTrans); @@ -99,8 +172,7 @@ public: void saveKinematicState(btScalar step); - - void applyForces(btScalar step); + void applyGravity(); void setGravity(const btVector3& acceleration); @@ -110,7 +182,9 @@ public: } void setDamping(btScalar lin_damping, btScalar ang_damping); - + + void applyDamping(btScalar timeStep); + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_collisionShape; } diff --git a/src/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 3bd7671ee..13c0548ed 100644 --- a/src/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/src/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -36,7 +36,6 @@ btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadpha :btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), m_constraintSolver(constraintSolver), m_ownsConstraintSolver(false), -m_debugDrawer(0), m_gravity(0,0,-10) { @@ -86,10 +85,27 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b synchronizeMotionStates(); + clearForces(); + return 1; } +void btSimpleDynamicsWorld::clearForces() +{ + //todo: iterate over awake simulation islands! + for ( int i=0;iclearForces(); + } + } +} + void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) { @@ -172,8 +188,9 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { if (body->isActive()) { - body->applyForces( timeStep); + body->applyGravity(); body->integrateVelocities( timeStep); + body->applyDamping(timeStep); body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); } } diff --git a/src/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/src/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index 4e38f74a7..5452455e2 100644 --- a/src/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/src/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -32,8 +32,6 @@ protected: bool m_ownsConstraintSolver; - btIDebugDraw* m_debugDrawer; - void predictUnconstraintMotion(btScalar timeStep); void integrateTransforms(btScalar timeStep); @@ -52,16 +50,6 @@ public: ///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); - virtual void setDebugDrawer(btIDebugDraw* debugDrawer) - { - m_debugDrawer = debugDrawer; - }; - - virtual btIDebugDraw* getDebugDrawer() - { - return m_debugDrawer; - } - virtual void setGravity(const btVector3& gravity); virtual void addRigidBody(btRigidBody* body); @@ -81,6 +69,8 @@ public: return BT_SIMPLE_DYNAMICS_WORLD; } + virtual void clearForces(); + }; #endif //BT_SIMPLE_DYNAMICS_WORLD_H diff --git a/src/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/src/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index 90e4e9962..ee5ad54e5 100644 --- a/src/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/src/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -23,8 +23,6 @@ #include "BulletDynamics/ConstraintSolver/btContactConstraint.h" - - static btRigidBody s_fixedObject( 0,0,0); btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) @@ -526,15 +524,10 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) if (!numWheel) return; - - void* mem = btAlignedAlloc(numWheel*sizeof(btVector3),16); - btVector3* forwardWS = new (mem)btVector3[numWheel]; - mem = btAlignedAlloc(numWheel*sizeof(btVector3),16); - btVector3* axle = new (mem)btVector3[numWheel]; - mem = btAlignedAlloc(numWheel*sizeof(btScalar),16); - btScalar* forwardImpulse = new (mem)btScalar[numWheel]; - mem = btAlignedAlloc(numWheel*sizeof(btScalar),16); - btScalar* sideImpulse = new(mem) btScalar[numWheel]; + m_forwardWS.resize(numWheel); + m_axle.resize(numWheel); + m_forwardImpulse.resize(numWheel); + m_sideImpulse.resize(numWheel); int numWheelsOnGround = 0; @@ -546,8 +539,8 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; if (groundObject) numWheelsOnGround++; - sideImpulse[i] = btScalar(0.); - forwardImpulse[i] = btScalar(0.); + m_sideImpulse[i] = btScalar(0.); + m_forwardImpulse[i] = btScalar(0.); } @@ -566,25 +559,25 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) const btTransform& wheelTrans = getWheelTransformWS( i ); btMatrix3x3 wheelBasis0 = wheelTrans.getBasis(); - axle[i] = btVector3( + m_axle[i] = btVector3( wheelBasis0[0][m_indexRightAxis], wheelBasis0[1][m_indexRightAxis], wheelBasis0[2][m_indexRightAxis]); const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; - btScalar proj = axle[i].dot(surfNormalWS); - axle[i] -= surfNormalWS * proj; - axle[i] = axle[i].normalize(); + btScalar proj = m_axle[i].dot(surfNormalWS); + m_axle[i] -= surfNormalWS * proj; + m_axle[i] = m_axle[i].normalize(); - forwardWS[i] = surfNormalWS.cross(axle[i]); - forwardWS[i].normalize(); + m_forwardWS[i] = surfNormalWS.cross(m_axle[i]); + m_forwardWS[i].normalize(); resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS, *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, - btScalar(0.), axle[i],sideImpulse[i],timeStep); + btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep); - sideImpulse[i] *= sideFrictionStiffness2; + m_sideImpulse[i] *= sideFrictionStiffness2; } @@ -613,7 +606,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) { btScalar defaultRollingFrictionImpulse = 0.f; btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; - btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,forwardWS[wheel],maxImpulse); + btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse); rollingFriction = calcRollingFriction(contactPt); } } @@ -623,7 +616,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) - forwardImpulse[wheel] = btScalar(0.); + m_forwardImpulse[wheel] = btScalar(0.); m_wheelInfo[wheel].m_skidInfo= btScalar(1.); if (groundObject) @@ -636,10 +629,10 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) btScalar maximpSquared = maximp * maximpSide; - forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep; + m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep; - btScalar x = (forwardImpulse[wheel] ) * fwdFactor; - btScalar y = (sideImpulse[wheel] ) * sideFactor; + btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor; + btScalar y = (m_sideImpulse[wheel] ) * sideFactor; btScalar impulseSquared = (x*x + y*y); @@ -663,12 +656,12 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) { for (int wheel = 0;wheel < getNumWheels(); wheel++) { - if (sideImpulse[wheel] != btScalar(0.)) + if (m_sideImpulse[wheel] != btScalar(0.)) { if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.)) { - forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; - sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; + m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; + m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; } } } @@ -683,11 +676,11 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - m_chassisBody->getCenterOfMassPosition(); - if (forwardImpulse[wheel] != btScalar(0.)) + if (m_forwardImpulse[wheel] != btScalar(0.)) { - m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos); + m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos); } - if (sideImpulse[wheel] != btScalar(0.)) + if (m_sideImpulse[wheel] != btScalar(0.)) { class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; @@ -695,7 +688,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) groundObject->getCenterOfMassPosition(); - btVector3 sideImp = axle[wheel] * sideImpulse[wheel]; + btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; rel_pos[2] *= wheelInfo.m_rollInfluence; m_chassisBody->applyImpulse(sideImp,rel_pos); @@ -706,10 +699,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) } } - btAlignedFree(forwardWS); - btAlignedFree(axle); - btAlignedFree(forwardImpulse); - btAlignedFree(sideImpulse); + } diff --git a/src/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/src/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h index a84b185e9..8361dcabe 100644 --- a/src/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/src/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -23,6 +23,12 @@ class btVehicleTuning; ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle. class btRaycastVehicle : public btTypedConstraint { + + btAlignedObjectArray m_forwardWS; + btAlignedObjectArray m_axle; + btAlignedObjectArray m_forwardImpulse; + btAlignedObjectArray m_sideImpulse; + public: class btVehicleTuning { @@ -114,7 +120,7 @@ public: void updateSuspension(btScalar deltaTime); - void updateFriction(btScalar timeStep); + virtual void updateFriction(btScalar timeStep); diff --git a/src/bullet/src/LinearMath/btAabbUtil2.h b/src/bullet/src/LinearMath/btAabbUtil2.h index 9b320961b..8bb6b3af7 100644 --- a/src/bullet/src/LinearMath/btAabbUtil2.h +++ b/src/bullet/src/LinearMath/btAabbUtil2.h @@ -20,7 +20,14 @@ subject to the following restrictions: #include "btVector3.h" #include "btMinMax.h" - +SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin, + btVector3& aabbMax, + const btVector3& expansionMin, + const btVector3& expansionMax) +{ + aabbMin = aabbMin + expansionMin; + aabbMax = aabbMax + expansionMax; +} /// conservative test for overlap between two aabbs @@ -65,6 +72,41 @@ SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent) } +SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, + const btVector3& rayInvDirection, + const unsigned int raySign[3], + const btVector3 bounds[2], + btScalar& tmin, + btScalar lambda_min, + btScalar lambda_max) +{ + btScalar tmax, tymin, tymax, tzmin, tzmax; + tmin = (bounds[raySign[0]][0] - rayFrom[0]) * rayInvDirection[0]; + tmax = (bounds[1-raySign[0]][0] - rayFrom[0]) * rayInvDirection[0]; + tymin = (bounds[raySign[1]][1] - rayFrom[1]) * rayInvDirection[1]; + tymax = (bounds[1-raySign[1]][1] - rayFrom[1]) * rayInvDirection[1]; + + if ( (tmin > tymax) || (tymin > tmax) ) + return false; + + if (tymin > tmin) + tmin = tymin; + + if (tymax < tmax) + tmax = tymax; + + tzmin = (bounds[raySign[2]][2] - rayFrom[2]) * rayInvDirection[2]; + tzmax = (bounds[1-raySign[2]][2] - rayFrom[2]) * rayInvDirection[2]; + + if ( (tmin > tzmax) || (tzmin > tmax) ) + return false; + if (tzmin > tmin) + tmin = tzmin; + if (tzmax < tmax) + tmax = tzmax; + return ( (tmin < lambda_max) && (tmax > lambda_min) ); +} + SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, const btVector3& rayTo, const btVector3& aabbMin, @@ -123,3 +165,4 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, #endif + diff --git a/src/bullet/src/LinearMath/btAlignedAllocator.cpp b/src/bullet/src/LinearMath/btAlignedAllocator.cpp index 40bb7422a..7dd0fa463 100644 --- a/src/bullet/src/LinearMath/btAlignedAllocator.cpp +++ b/src/bullet/src/LinearMath/btAlignedAllocator.cpp @@ -32,7 +32,7 @@ void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filen gTotalBytesAlignedAllocs += size; gNumAlignedAllocs++; - printf("allocation#%d from %s,line %d, size %d\n",gNumAlignedAllocs,filename,line,size); + real = (char *)malloc(size + 2*sizeof(void *) + (alignment-1)); if (real) { offset = (alignment - (unsigned long)(real + 2*sizeof(void *))) & @@ -44,6 +44,9 @@ void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filen } else { ret = (void *)(real);//?? } + + printf("allocation#%d at address %x, from %s,line %d, size %d\n",gNumAlignedAllocs,real, filename,line,size); + int* ptr = (int*)ret; *ptr = 12; return (ret); @@ -60,7 +63,7 @@ void btAlignedFreeInternal (void* ptr,int line,char* filename) int size = *((int*)(ptr)-2); gTotalBytesAlignedAllocs -= size; - printf("free #%d from %s,line %d, size %d\n",gNumAlignedFree,filename,line,size); + printf("free #%d at address %x, from %s,line %d, size %d\n",gNumAlignedFree,real, filename,line,size); free(real); } else diff --git a/src/bullet/src/LinearMath/btPoolAllocator.h b/src/bullet/src/LinearMath/btPoolAllocator.h index 69105dc87..ce06e1d38 100644 --- a/src/bullet/src/LinearMath/btPoolAllocator.h +++ b/src/bullet/src/LinearMath/btPoolAllocator.h @@ -77,7 +77,7 @@ public: return false; } - void free(void* ptr) + void freeMemory(void* ptr) { if (ptr) { btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize); diff --git a/src/bullet/src/LinearMath/btQuickprof.cpp b/src/bullet/src/LinearMath/btQuickprof.cpp index 37a0c8c3b..364b5a575 100644 --- a/src/bullet/src/LinearMath/btQuickprof.cpp +++ b/src/bullet/src/LinearMath/btQuickprof.cpp @@ -1,38 +1,268 @@ /* -Copyright (c) 2006 Tyler Streeter -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. - -*/ - - -// Please visit the project website (http://quickprof.sourceforge.net) -// for usage instructions. +/*************************************************************************************************** +** +** profile.cpp +** +** Real-Time Hierarchical Profiling for Game Programming Gems 3 +** +** by Greg Hjelstrom & Byon Garrabrant +** +***************************************************************************************************/ // Credits: The Clock class was inspired by the Timer classes in // Ogre (www.ogre3d.org). #include "LinearMath/btQuickprof.h" -#ifdef USE_QUICKPROF +static btClock gProfileClock; + +inline void Profile_Get_Ticks(unsigned long int * ticks) +{ + *ticks = gProfileClock.getTimeMicroseconds(); +} + +inline float Profile_Get_Tick_Rate(void) +{ +// return 1000000.f; + return 1000.f; + +} + +/*************************************************************************************************** +** +** CProfileNode +** +***************************************************************************************************/ + +/*********************************************************************************************** + * INPUT: * + * name - pointer to a static string which is the name of this profile node * + * parent - parent pointer * + * * + * WARNINGS: * + * The name is assumed to be a static pointer, only the pointer is stored and compared for * + * efficiency reasons. * + *=============================================================================================*/ +CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) : + Name( name ), + TotalCalls( 0 ), + TotalTime( 0 ), + StartTime( 0 ), + RecursionCounter( 0 ), + Parent( parent ), + Child( NULL ), + Sibling( NULL ) +{ + Reset(); +} + + +CProfileNode::~CProfileNode( void ) +{ + delete Child; + delete Sibling; +} + + +/*********************************************************************************************** + * INPUT: * + * name - static string pointer to the name of the node we are searching for * + * * + * WARNINGS: * + * All profile names are assumed to be static strings so this function uses pointer compares * + * to find the named node. * + *=============================================================================================*/ +CProfileNode * CProfileNode::Get_Sub_Node( const char * name ) +{ + // Try to find this sub node + CProfileNode * child = Child; + while ( child ) { + if ( child->Name == name ) { + return child; + } + child = child->Sibling; + } + + // We didn't find it, so add it + CProfileNode * node = new CProfileNode( name, this ); + node->Sibling = Child; + Child = node; + return node; +} + + +void CProfileNode::Reset( void ) +{ + TotalCalls = 0; + TotalTime = 0.0f; + gProfileClock.reset(); + + if ( Child ) { + Child->Reset(); + } + if ( Sibling ) { + Sibling->Reset(); + } +} + + +void CProfileNode::Call( void ) +{ + TotalCalls++; + if (RecursionCounter++ == 0) { + Profile_Get_Ticks(&StartTime); + } +} + + +bool CProfileNode::Return( void ) +{ + if ( --RecursionCounter == 0 && TotalCalls != 0 ) { + unsigned long int time; + Profile_Get_Ticks(&time); + time-=StartTime; + TotalTime += (float)time / Profile_Get_Tick_Rate(); + } + return ( RecursionCounter == 0 ); +} + + +/*************************************************************************************************** +** +** CProfileIterator +** +***************************************************************************************************/ +CProfileIterator::CProfileIterator( CProfileNode * start ) +{ + CurrentParent = start; + CurrentChild = CurrentParent->Get_Child(); +} + + +void CProfileIterator::First(void) +{ + CurrentChild = CurrentParent->Get_Child(); +} + + +void CProfileIterator::Next(void) +{ + CurrentChild = CurrentChild->Get_Sibling(); +} + + +bool CProfileIterator::Is_Done(void) +{ + return CurrentChild == NULL; +} + + +void CProfileIterator::Enter_Child( int index ) +{ + CurrentChild = CurrentParent->Get_Child(); + while ( (CurrentChild != NULL) && (index != 0) ) { + index--; + CurrentChild = CurrentChild->Get_Sibling(); + } + + if ( CurrentChild != NULL ) { + CurrentParent = CurrentChild; + CurrentChild = CurrentParent->Get_Child(); + } +} + + +void CProfileIterator::Enter_Parent( void ) +{ + if ( CurrentParent->Get_Parent() != NULL ) { + CurrentParent = CurrentParent->Get_Parent(); + } + CurrentChild = CurrentParent->Get_Child(); +} + + +/*************************************************************************************************** +** +** CProfileManager +** +***************************************************************************************************/ + +CProfileNode CProfileManager::Root( "Root", NULL ); +CProfileNode * CProfileManager::CurrentNode = &CProfileManager::Root; +int CProfileManager::FrameCounter = 0; +unsigned long int CProfileManager::ResetTime = 0; + + +/*********************************************************************************************** + * CProfileManager::Start_Profile -- Begin a named profile * + * * + * Steps one level deeper into the tree, if a child already exists with the specified name * + * then it accumulates the profiling; otherwise a new child node is added to the profile tree. * + * * + * INPUT: * + * name - name of this profiling record * + * * + * WARNINGS: * + * The string used is assumed to be a static string; pointer compares are used throughout * + * the profiling code for efficiency. * + *=============================================================================================*/ +void CProfileManager::Start_Profile( const char * name ) +{ + if (name != CurrentNode->Get_Name()) { + CurrentNode = CurrentNode->Get_Sub_Node( name ); + } + + CurrentNode->Call(); +} + + +/*********************************************************************************************** + * CProfileManager::Stop_Profile -- Stop timing and record the results. * + *=============================================================================================*/ +void CProfileManager::Stop_Profile( void ) +{ + // Return will indicate whether we should back up to our parent (we may + // be profiling a recursive function) + if (CurrentNode->Return()) { + CurrentNode = CurrentNode->Get_Parent(); + } +} + + +/*********************************************************************************************** + * CProfileManager::Reset -- Reset the contents of the profiling system * + * * + * This resets everything except for the tree structure. All of the timing data is reset. * + *=============================================================================================*/ +void CProfileManager::Reset( void ) +{ + Root.Reset(); + Root.Call(); + FrameCounter = 0; + Profile_Get_Ticks(&ResetTime); +} + + +/*********************************************************************************************** + * CProfileManager::Increment_Frame_Counter -- Increment the frame counter * + *=============================================================================================*/ +void CProfileManager::Increment_Frame_Counter( void ) +{ + FrameCounter++; +} + + +/*********************************************************************************************** + * CProfileManager::Get_Time_Since_Reset -- returns the elapsed time since last reset * + *=============================================================================================*/ +float CProfileManager::Get_Time_Since_Reset( void ) +{ + unsigned long int time; + Profile_Get_Ticks(&time); + time -= ResetTime; + return (float)time / Profile_Get_Tick_Rate(); +} + + -// Note: We must declare these private static variables again here to -// avoid link errors. -bool btProfiler::mEnabled = false; -btClock btProfiler::mClock; -unsigned long int btProfiler::mCurrentCycleStartMicroseconds = 0; -unsigned long int btProfiler::mLastCycleDurationMicroseconds = 0; -std::map btProfiler::mProfileBlocks; -std::ofstream btProfiler::mOutputFile; -bool btProfiler::mFirstFileOutput = true; -btProfiler::BlockTimingMethod btProfiler::mFileOutputMethod; -unsigned long int btProfiler::mCycleNumber = 0; -#endif //USE_QUICKPROF diff --git a/src/bullet/src/LinearMath/btQuickprof.h b/src/bullet/src/LinearMath/btQuickprof.h index a885967c5..35a87cece 100644 --- a/src/bullet/src/LinearMath/btQuickprof.h +++ b/src/bullet/src/LinearMath/btQuickprof.h @@ -1,20 +1,11 @@ -/* -Copyright (c) 2006 Tyler Streeter - -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. - -*/ -// Please visit the project website (http://quickprof.sourceforge.net) -// for usage instructions. +/*************************************************************************************************** +** +** Real-Time Hierarchical Profiling for Game Programming Gems 3 +** +** by Greg Hjelstrom & Byon Garrabrant +** +***************************************************************************************************/ // Credits: The Clock class was inspired by the Timer classes in // Ogre (www.ogre3d.org). @@ -24,8 +15,8 @@ subject to the following restrictions: #include "btScalar.h" -//#define USE_QUICKPROF 1 -//Don't use quickprof for now, because it contains STL. TODO: replace STL by Bullet container classes. +//To disable built-in profiling, please comment out next line +//#define BT_NO_PROFILE 1 //if you don't need btClock, you can comment next line @@ -235,477 +226,129 @@ class btClock #endif //USE_BT_CLOCK -#ifdef USE_QUICKPROF -#include -#include -#include -#include +/* +** A node in the Profile Hierarchy Tree +*/ +class CProfileNode { +public: + CProfileNode( const char * name, CProfileNode * parent ); + ~CProfileNode( void ); + CProfileNode * Get_Sub_Node( const char * name ); + CProfileNode * Get_Parent( void ) { return Parent; } + CProfileNode * Get_Sibling( void ) { return Sibling; } + CProfileNode * Get_Child( void ) { return Child; } -namespace hidden -{ - /// A simple data structure representing a single timed block - /// of code. - struct ProfileBlock - { - ProfileBlock() - { - currentBlockStartMicroseconds = 0; - currentCycleTotalMicroseconds = 0; - lastCycleTotalMicroseconds = 0; - totalMicroseconds = 0; - } + void Reset( void ); + void Call( void ); + bool Return( void ); - /// The starting time (in us) of the current block update. - unsigned long int currentBlockStartMicroseconds; + const char * Get_Name( void ) { return Name; } + int Get_Total_Calls( void ) { return TotalCalls; } + float Get_Total_Time( void ) { return TotalTime; } - /// The accumulated time (in us) spent in this block during the - /// current profiling cycle. - unsigned long int currentCycleTotalMicroseconds; +protected: - /// The accumulated time (in us) spent in this block during the - /// past profiling cycle. - unsigned long int lastCycleTotalMicroseconds; - - /// The total accumulated time (in us) spent in this block. - unsigned long int totalMicroseconds; - }; + const char * Name; + int TotalCalls; + float TotalTime; + unsigned long int StartTime; + int RecursionCounter; + CProfileNode * Parent; + CProfileNode * Child; + CProfileNode * Sibling; }; -/// A static class that manages timing for a set of profiling blocks. -class btProfiler +/* +** An iterator to navigate through the tree +*/ +class CProfileIterator { public: - /// A set of ways to retrieve block timing data. - enum BlockTimingMethod - { - /// The total time spent in the block (in seconds) since the - /// profiler was initialized. - BLOCK_TOTAL_SECONDS, + // Access all the children of the current parent + void First(void); + void Next(void); + bool Is_Done(void); + bool Is_Root(void) { return (CurrentParent->Get_Parent() == 0); } - /// The total time spent in the block (in ms) since the - /// profiler was initialized. - BLOCK_TOTAL_MILLISECONDS, + void Enter_Child( int index ); // Make the given child the new parent + void Enter_Largest_Child( void ); // Make the largest child the new parent + void Enter_Parent( void ); // Make the current parent's parent the new parent - /// The total time spent in the block (in us) since the - /// profiler was initialized. - BLOCK_TOTAL_MICROSECONDS, + // Access the current child + const char * Get_Current_Name( void ) { return CurrentChild->Get_Name(); } + int Get_Current_Total_Calls( void ) { return CurrentChild->Get_Total_Calls(); } + float Get_Current_Total_Time( void ) { return CurrentChild->Get_Total_Time(); } - /// The total time spent in the block, as a % of the total - /// elapsed time since the profiler was initialized. - BLOCK_TOTAL_PERCENT, + // Access the current parent + const char * Get_Current_Parent_Name( void ) { return CurrentParent->Get_Name(); } + int Get_Current_Parent_Total_Calls( void ) { return CurrentParent->Get_Total_Calls(); } + float Get_Current_Parent_Total_Time( void ) { return CurrentParent->Get_Total_Time(); } - /// The time spent in the block (in seconds) in the most recent - /// profiling cycle. - BLOCK_CYCLE_SECONDS, +protected: - /// The time spent in the block (in ms) in the most recent - /// profiling cycle. - BLOCK_CYCLE_MILLISECONDS, + CProfileNode * CurrentParent; + CProfileNode * CurrentChild; - /// The time spent in the block (in us) in the most recent - /// profiling cycle. - BLOCK_CYCLE_MICROSECONDS, - - /// The time spent in the block (in seconds) in the most recent - /// profiling cycle, as a % of the total cycle time. - BLOCK_CYCLE_PERCENT - }; - - /// Initializes the profiler. This must be called first. If this is - /// never called, the profiler is effectively disabled; all other - /// functions will return immediately. The first parameter - /// is the name of an output data file; if this string is not empty, - /// data will be saved on every profiling cycle; if this string is - /// empty, no data will be saved to a file. The second parameter - /// determines which timing method is used when printing data to the - /// output file. - inline static void init(const std::string outputFilename="", - BlockTimingMethod outputMethod=BLOCK_CYCLE_MILLISECONDS); - - /// Cleans up allocated memory. - inline static void destroy(); - - /// Begins timing the named block of code. - inline static void beginBlock(const std::string& name); - - /// Updates the accumulated time spent in the named block by adding - /// the elapsed time since the last call to startBlock for this block - /// name. - inline static void endBlock(const std::string& name); - - /// Returns the time spent in the named block according to the - /// given timing method. See comments on BlockTimingMethod for details. - inline static double getBlockTime(const std::string& name, - BlockTimingMethod method=BLOCK_CYCLE_MILLISECONDS); - - /// Defines the end of a profiling cycle. Use this regularly if you - /// want to generate detailed timing information. This must not be - /// called within a timing block. - inline static void endProfilingCycle(); - - /// A helper function that creates a string of statistics for - /// each timing block. This is mainly for printing an overall - /// summary to the command line. - inline static std::string createStatsString( - BlockTimingMethod method=BLOCK_TOTAL_PERCENT); - -//private: - inline btProfiler(); - - inline ~btProfiler(); - - /// Prints an error message to standard output. - inline static void printError(const std::string& msg) - { - //btAssert(0); - std::cout << "[QuickProf error] " << msg << std::endl; - } - - /// Determines whether the profiler is enabled. - static bool mEnabled; - - /// The clock used to time profile blocks. - static btClock mClock; - - /// The starting time (in us) of the current profiling cycle. - static unsigned long int mCurrentCycleStartMicroseconds; - - /// The duration (in us) of the most recent profiling cycle. - static unsigned long int mLastCycleDurationMicroseconds; - - /// Internal map of named profile blocks. - static std::map mProfileBlocks; - - /// The data file used if this feature is enabled in 'init.' - static std::ofstream mOutputFile; - - /// Tracks whether we have begun print data to the output file. - static bool mFirstFileOutput; - - /// The method used when printing timing data to an output file. - static BlockTimingMethod mFileOutputMethod; - - /// The number of the current profiling cycle. - static unsigned long int mCycleNumber; + CProfileIterator( CProfileNode * start ); + friend class CProfileManager; }; -btProfiler::btProfiler() -{ - // This never gets called because a btProfiler instance is never - // created. -} +/* +** The Manager for the Profile system +*/ +class CProfileManager { +public: + static void Start_Profile( const char * name ); + static void Stop_Profile( void ); -btProfiler::~btProfiler() -{ - // This never gets called because a btProfiler instance is never - // created. -} + static void Reset( void ); + static void Increment_Frame_Counter( void ); + static int Get_Frame_Count_Since_Reset( void ) { return FrameCounter; } + static float Get_Time_Since_Reset( void ); -void btProfiler::init(const std::string outputFilename, - BlockTimingMethod outputMethod) -{ - mEnabled = true; + static CProfileIterator * Get_Iterator( void ) { return new CProfileIterator( &Root ); } + static void Release_Iterator( CProfileIterator * iterator ) { delete iterator; } + +private: + static CProfileNode Root; + static CProfileNode * CurrentNode; + static int FrameCounter; + static unsigned long int ResetTime; +}; + + +/* +** ProfileSampleClass is a simple way to profile a function's scope +** Use the BT_PROFILE macro at the start of scope to time +*/ +class CProfileSample { +public: + CProfileSample( const char * name ) + { + CProfileManager::Start_Profile( name ); + } - if (!outputFilename.empty()) - { - mOutputFile.open(outputFilename.c_str()); + ~CProfileSample( void ) + { + CProfileManager::Stop_Profile(); } +}; - mFileOutputMethod = outputMethod; - - mClock.reset(); - - // Set the start time for the first cycle. - mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds(); -} - -void btProfiler::destroy() -{ - if (!mEnabled) - { - return; - } - - if (mOutputFile.is_open()) - { - mOutputFile.close(); - } - - // Destroy all ProfileBlocks. - while (!mProfileBlocks.empty()) - { - delete (*mProfileBlocks.begin()).second; - mProfileBlocks.erase(mProfileBlocks.begin()); - } -} - -void btProfiler::beginBlock(const std::string& name) -{ - if (!mEnabled) - { - return; - } - - if (name.empty()) - { - printError("Cannot allow unnamed profile blocks."); - return; - } - - hidden::ProfileBlock* block = mProfileBlocks[name]; - - if (!block) - { - // Create a new ProfileBlock. - mProfileBlocks[name] = new hidden::ProfileBlock(); - block = mProfileBlocks[name]; - } - - // We do this at the end to get more accurate results. - block->currentBlockStartMicroseconds = mClock.getTimeMicroseconds(); -} - -void btProfiler::endBlock(const std::string& name) -{ - if (!mEnabled) - { - return; - } - - // We do this at the beginning to get more accurate results. - unsigned long int endTick = mClock.getTimeMicroseconds(); - - hidden::ProfileBlock* block = mProfileBlocks[name]; - - if (!block) - { - // The named block does not exist. Print an error. - printError("The profile block named '" + name + - "' does not exist."); - return; - } - - unsigned long int blockDuration = endTick - - block->currentBlockStartMicroseconds; - block->currentCycleTotalMicroseconds += blockDuration; - block->totalMicroseconds += blockDuration; -} - -double btProfiler::getBlockTime(const std::string& name, - BlockTimingMethod method) -{ - if (!mEnabled) - { - return 0; - } - - hidden::ProfileBlock* block = mProfileBlocks[name]; - - if (!block) - { - // The named block does not exist. Print an error. - printError("The profile block named '" + name + - "' does not exist."); - return 0; - } - - double result = 0; - - switch(method) - { - case BLOCK_TOTAL_SECONDS: - result = (double)block->totalMicroseconds * (double)0.000001; - break; - case BLOCK_TOTAL_MILLISECONDS: - result = (double)block->totalMicroseconds * (double)0.001; - break; - case BLOCK_TOTAL_MICROSECONDS: - result = (double)block->totalMicroseconds; - break; - case BLOCK_TOTAL_PERCENT: - { - double timeSinceInit = (double)mClock.getTimeMicroseconds(); - if (timeSinceInit <= 0) - { - result = 0; - } - else - { - result = 100.0 * (double)block->totalMicroseconds / - timeSinceInit; - } - break; - } - case BLOCK_CYCLE_SECONDS: - result = (double)block->lastCycleTotalMicroseconds * - (double)0.000001; - break; - case BLOCK_CYCLE_MILLISECONDS: - result = (double)block->lastCycleTotalMicroseconds * - (double)0.001; - break; - case BLOCK_CYCLE_MICROSECONDS: - result = (double)block->lastCycleTotalMicroseconds; - break; - case BLOCK_CYCLE_PERCENT: - { - if (0 == mLastCycleDurationMicroseconds) - { - // We have not yet finished a cycle, so just return zero - // percent to avoid a divide by zero error. - result = 0; - } - else - { - result = 100.0 * (double)block->lastCycleTotalMicroseconds / - mLastCycleDurationMicroseconds; - } - break; - } - default: - break; - } - - return result; -} - -void btProfiler::endProfilingCycle() -{ - if (!mEnabled) - { - return; - } - - // Store the duration of the cycle that just finished. - mLastCycleDurationMicroseconds = mClock.getTimeMicroseconds() - - mCurrentCycleStartMicroseconds; - - // For each block, update data for the cycle that just finished. - std::map::iterator iter; - for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); ++iter) - { - hidden::ProfileBlock* block = (*iter).second; - block->lastCycleTotalMicroseconds = - block->currentCycleTotalMicroseconds; - block->currentCycleTotalMicroseconds = 0; - } - - if (mOutputFile.is_open()) - { - // Print data to the output file. - if (mFirstFileOutput) - { - // On the first iteration, print a header line that shows the - // names of each profiling block. - mOutputFile << "#cycle, "; - - for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); - ++iter) - { - mOutputFile << (*iter).first << ", "; - } - - mOutputFile << std::endl; - mFirstFileOutput = false; - } - - mOutputFile << mCycleNumber << ", "; - - for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); - ++iter) - { - mOutputFile << getBlockTime((*iter).first, mFileOutputMethod) - << ", "; - } - - mOutputFile << std::endl; - } - - ++mCycleNumber; - mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds(); -} - -std::string btProfiler::createStatsString(BlockTimingMethod method) -{ - if (!mEnabled) - { - return ""; - } - - std::string s; - std::string suffix; - - switch(method) - { - case BLOCK_TOTAL_SECONDS: - suffix = "s"; - break; - case BLOCK_TOTAL_MILLISECONDS: - suffix = "ms"; - break; - case BLOCK_TOTAL_MICROSECONDS: - suffix = "us"; - break; - case BLOCK_TOTAL_PERCENT: - { - suffix = "%"; - break; - } - case BLOCK_CYCLE_SECONDS: - suffix = "s"; - break; - case BLOCK_CYCLE_MILLISECONDS: - suffix = "ms"; - break; - case BLOCK_CYCLE_MICROSECONDS: - suffix = "us"; - break; - case BLOCK_CYCLE_PERCENT: - { - suffix = "%"; - break; - } - default: - break; - } - - std::map::iterator iter; - for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); ++iter) - { - if (iter != mProfileBlocks.begin()) - { - s += "\n"; - } - - char blockTime[64]; - sprintf(blockTime, "%lf", getBlockTime((*iter).first, method)); - - s += (*iter).first; - s += ": "; - s += blockTime; - s += " "; - s += suffix; - } - - return s; -} +#if !defined(BT_NO_PROFILE) +#define BT_PROFILE( name ) CProfileSample __profile( name ) +#else +#define BT_PROFILE( name ) +#endif -#define BEGIN_PROFILE(a) btProfiler::beginBlock(a) -#define END_PROFILE(a) btProfiler::endBlock(a) -#else //USE_QUICKPROF -#define BEGIN_PROFILE(a) -#define END_PROFILE(a) - -#endif //USE_QUICKPROF #endif //QUICK_PROF_H diff --git a/src/bullet/src/LinearMath/btScalar.h b/src/bullet/src/LinearMath/btScalar.h index 85dfaf3eb..63fb4e8b3 100644 --- a/src/bullet/src/LinearMath/btScalar.h +++ b/src/bullet/src/LinearMath/btScalar.h @@ -23,12 +23,18 @@ subject to the following restrictions: #include #include +#define BT_BULLET_VERSION 266 inline int btGetVersion() { - return 264; + return BT_BULLET_VERSION; } +#if defined(DEBUG) || defined (_DEBUG) +#define BT_DEBUG +#endif + + #ifdef WIN32 #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) @@ -110,6 +116,8 @@ inline int btGetVersion() #define btAssert assert //btFullAssert is optional, slows down a lot #define btFullAssert(x) + #define btLikely(_c) _c + #define btUnlikely(_c) _c #endif // LIBSPE2 diff --git a/src/bullet/src/Makefile.am b/src/bullet/src/Makefile.am index 81a5318e4..4a592f09b 100644 --- a/src/bullet/src/Makefile.am +++ b/src/bullet/src/Makefile.am @@ -41,6 +41,8 @@ libbulletcollision_a_SOURCES := \ BulletCollision/CollisionDispatch/btCollisionWorld.h \ BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp \ BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h \ + BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp \ + BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm..h \ BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp \ BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h \ BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp \ diff --git a/src/ide/vc8/bullet_lib.vcproj b/src/ide/vc8/bullet_lib.vcproj index fa68c4583..790333629 100755 --- a/src/ide/vc8/bullet_lib.vcproj +++ b/src/ide/vc8/bullet_lib.vcproj @@ -226,6 +226,10 @@ RelativePath="..\..\bullet\src\BulletCollision\CollisionDispatch\btConvexConvexAlgorithm.cpp" > + + @@ -744,6 +748,10 @@ RelativePath="..\..\bullet\src\BulletCollision\CollisionDispatch\btConvexConvexAlgorithm.h" > + +