diff options
author | Erwin Coumans <blender@erwincoumans.com> | 2006-11-13 00:05:10 +0300 |
---|---|---|
committer | Erwin Coumans <blender@erwincoumans.com> | 2006-11-13 00:05:10 +0300 |
commit | 3a1b7ece402001f2f93669a7b36c32f5e7827bab (patch) | |
tree | a91e600407b0709922b7df3fa2a53f380cf7eb76 /extern | |
parent | 22d97b2e346e3cb3fc38704a0460e2dd4d9a0abb (diff) |
updating Bullet 2.x with latest changes. The integration + C-API will follow at some stage.
Diffstat (limited to 'extern')
57 files changed, 1544 insertions, 517 deletions
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index 3462e6e472d..7b35df09db6 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -44,7 +44,9 @@ CONCAVE_SHAPES_START_HERE, TRIANGLE_MESH_SHAPE_PROXYTYPE, ///used for demo integration FAST/Swift collision library and Bullet FAST_CONCAVE_MESH_PROXYTYPE, - +///Used for GIMPACT Trimesh integration + GIMPACT_SHAPE_PROXYTYPE, + EMPTY_SHAPE_PROXYTYPE, STATIC_PLANE_PROXYTYPE, CONCAVE_SHAPES_END_HERE, diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h index 3195f996fe0..f9e22057058 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -19,7 +19,7 @@ subject to the following restrictions: struct btBroadphaseProxy; class btDispatcher; class btManifoldResult; -struct btCollisionObject; +class btCollisionObject; struct btDispatcherInfo; class btPersistentManifold; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 75ef338fdde..bbc5bd3f5d0 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -19,7 +19,7 @@ subject to the following restrictions: class btCollisionAlgorithm; struct btBroadphaseProxy; class btRigidBody; -struct btCollisionObject; +class btCollisionObject; class btOverlappingPairCache; enum btCollisionDispatcherId @@ -42,7 +42,8 @@ struct btDispatcherInfo m_timeOfImpact(1.f), m_useContinuous(false), m_debugDraw(0), - m_enableSatConvex(false) + m_enableSatConvex(false), + m_enableSPU(false) { } @@ -53,6 +54,7 @@ struct btDispatcherInfo bool m_useContinuous; class btIDebugDraw* m_debugDraw; bool m_enableSatConvex; + bool m_enableSPU; }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index 1a24c7a7a8c..6281e93eeb4 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -101,6 +101,7 @@ protected: virtual bool processOverlap(btBroadphasePair& pair) { assert(0); + return false; } }; diff --git a/extern/bullet2/src/BulletCollision/CMakeLists.txt b/extern/bullet2/src/BulletCollision/CMakeLists.txt index d10bd249149..3169f15199d 100644 --- a/extern/bullet2/src/BulletCollision/CMakeLists.txt +++ b/extern/bullet2/src/BulletCollision/CMakeLists.txt @@ -42,6 +42,7 @@ ADD_LIBRARY(LibBulletCollision CollisionShapes/btStaticPlaneShape.cpp CollisionShapes/btStridingMeshInterface.cpp CollisionShapes/btTriangleCallback.cpp + CollisionShapes/btTriangleBuffer.cpp CollisionShapes/btTriangleIndexVertexArray.cpp CollisionShapes/btTriangleMesh.cpp CollisionShapes/btTriangleMeshShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index 6d499f508e9..1eaa4a0fd50 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -18,9 +18,9 @@ subject to the following restrictions: #include <vector> -typedef std::vector<struct btCollisionObject*> btCollisionObjectArray; +typedef std::vector<class btCollisionObject*> btCollisionObjectArray; class btCollisionAlgorithm; -struct btCollisionObject; +class btCollisionObject; struct btCollisionAlgorithmConstructionInfo; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index ebfccef9cb1..c9d21f2d9a3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -14,6 +14,7 @@ subject to the following restrictions: */ + #include "btCollisionDispatcher.h" @@ -34,8 +35,8 @@ int gNumManifold = 0; btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms) :m_useIslands(true), -m_count(0), m_convexConvexCreateFunc(0), +m_count(0), m_convexConcaveCreateFunc(0), m_swappedConvexConcaveCreateFunc(0), m_compoundCreateFunc(0), @@ -54,7 +55,7 @@ m_emptyCreateFunc(0) } } - + btCollisionDispatcher::btCollisionDispatcher (): m_useIslands(true), m_count(0) @@ -150,7 +151,7 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher = this; ci.m_manifold = sharedManifold; - btCollisionAlgorithm* algo = m_doubleDispatch[body0->m_collisionShape->getShapeType()][body1->m_collisionShape->getShapeType()] + btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()] ->CreateCollisionAlgorithm(ci,body0,body1); #else btCollisionAlgorithm* algo = internalFindAlgorithm(body0,body1); @@ -201,27 +202,27 @@ btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionOb btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher = this; - if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConvex() ) + if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConvex() ) { return new btConvexConvexAlgorithm(sharedManifold,ci,body0,body1); } - if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConcave()) + if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConcave()) { return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false); } - if (body1->m_collisionShape->isConvex() && body0->m_collisionShape->isConcave()) + if (body1->getCollisionShape()->isConvex() && body0->getCollisionShape()->isConcave()) { return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true); } - if (body0->m_collisionShape->isCompound()) + if (body0->getCollisionShape()->isCompound()) { return new btCompoundCollisionAlgorithm(ci,body0,body1,false); } else { - if (body1->m_collisionShape->isCompound()) + if (body1->getCollisionShape()->isCompound()) { return new btCompoundCollisionAlgorithm(ci,body0,body1,true); } @@ -257,7 +258,7 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); } - if ((!body0->IsActive()) && (!body1->IsActive())) + if ((!body0->isActive()) && (!body1->isActive())) needsCollision = false; return needsCollision ; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 4179bc47e82..9cd2828dcad 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -30,22 +30,22 @@ btCollisionObject::btCollisionObject() } -void btCollisionObject::SetActivationState(int newState) +void btCollisionObject::setActivationState(int newState) { if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION)) m_activationState1 = newState; } -void btCollisionObject::ForceActivationState(int newState) +void btCollisionObject::forceActivationState(int newState) { m_activationState1 = newState; } void btCollisionObject::activate() { - if (!(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OJBECT))) + if (!(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT))) { - SetActivationState(ACTIVE_TAG); + setActivationState(ACTIVE_TAG); m_deactivationTime = 0.f; } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 5df3de489cd..b85534c1d15 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -34,8 +34,11 @@ class btCollisionShape; /// btCollisionObject can be used to manage collision detection objects. /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. /// They can be added to the btCollisionWorld. -struct btCollisionObject +class btCollisionObject { + +protected: + btTransform m_worldTransform; btBroadphaseProxy* m_broadphaseHandle; btCollisionShape* m_collisionShape; @@ -48,15 +51,6 @@ struct btCollisionObject btVector3 m_interpolationLinearVelocity; btVector3 m_interpolationAngularVelocity; - - enum CollisionFlags - { - CF_STATIC_OBJECT= 1, - CF_KINEMATIC_OJBECT= 2, - CF_NO_CONTACT_RESPONSE = 4, - CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) - }; - int m_collisionFlags; int m_islandTag1; @@ -66,7 +60,7 @@ struct btCollisionObject btScalar m_friction; btScalar m_restitution; - ///users can point to their objects, m_userPointer is not used by Bullet + ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer void* m_userObjectPointer; ///m_internalOwner is reserved to point to Bullet's btRigidBody. Don't use this, use m_userObjectPointer instead. @@ -81,10 +75,21 @@ struct btCollisionObject /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold float m_ccdSquareMotionThreshold; +public: + + enum CollisionFlags + { + CF_STATIC_OBJECT= 1, + CF_KINEMATIC_OBJECT= 2, + CF_NO_CONTACT_RESPONSE = 4, + CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) + }; + + inline bool mergesSimulationIslands() const { ///static objects, kinematic and object without contact response don't merge islands - return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OJBECT | CF_NO_CONTACT_RESPONSE) )==0); + return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0); } @@ -94,12 +99,12 @@ struct btCollisionObject inline bool isKinematicObject() const { - return (m_collisionFlags & CF_KINEMATIC_OJBECT) != 0; + return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0; } inline bool isStaticOrKinematicObject() const { - return (m_collisionFlags & (CF_KINEMATIC_OJBECT | CF_STATIC_OBJECT)) != 0 ; + return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ; } inline bool hasContactResponse() const { @@ -107,27 +112,47 @@ struct btCollisionObject } - - btCollisionObject(); - void SetCollisionShape(btCollisionShape* collisionShape) + void setCollisionShape(btCollisionShape* collisionShape) { m_collisionShape = collisionShape; } - int GetActivationState() const { return m_activationState1;} + const btCollisionShape* getCollisionShape() const + { + return m_collisionShape; + } + + btCollisionShape* getCollisionShape() + { + return m_collisionShape; + } + - void SetActivationState(int newState); - void ForceActivationState(int newState); + + int getActivationState() const { return m_activationState1;} + + void setActivationState(int newState); + + void setDeactivationTime(float time) + { + m_deactivationTime = time; + } + float getDeactivationTime() const + { + return m_deactivationTime; + } + + void forceActivationState(int newState); void activate(); - inline bool IsActive() const + inline bool isActive() const { - return ((GetActivationState() != ISLAND_SLEEPING) && (GetActivationState() != DISABLE_SIMULATION)); + return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } void setRestitution(float rest) @@ -147,6 +172,141 @@ struct btCollisionObject return m_friction; } + ///reserved for Bullet internal usage + void* getInternalOwner() + { + return m_internalOwner; + } + + const void* getInternalOwner() const + { + return m_internalOwner; + } + + btTransform& getWorldTransform() + { + return m_worldTransform; + } + + const btTransform& getWorldTransform() const + { + return m_worldTransform; + } + + void setWorldTransform(const btTransform& worldTrans) + { + m_worldTransform = worldTrans; + } + + + btBroadphaseProxy* getBroadphaseHandle() + { + return m_broadphaseHandle; + } + + const btBroadphaseProxy* getBroadphaseHandle() const + { + return m_broadphaseHandle; + } + + void setBroadphaseHandle(btBroadphaseProxy* handle) + { + m_broadphaseHandle = handle; + } + + + const btTransform& getInterpolationWorldTransform() const + { + return m_interpolationWorldTransform; + } + + btTransform& getInterpolationWorldTransform() + { + return m_interpolationWorldTransform; + } + + void setInterpolationWorldTransform(const btTransform& trans) + { + m_interpolationWorldTransform = trans; + } + + + const btVector3& getInterpolationLinearVelocity() const + { + return m_interpolationLinearVelocity; + } + + const btVector3& getInterpolationAngularVelocity() const + { + return m_interpolationAngularVelocity; + } + + const int getIslandTag() const + { + return m_islandTag1; + } + + void setIslandTag(int tag) + { + m_islandTag1 = tag; + } + + const float getHitFraction() const + { + return m_hitFraction; + } + + void setHitFraction(float hitFraction) + { + m_hitFraction = hitFraction; + } + + + const int getCollisionFlags() const + { + return m_collisionFlags; + } + + void setCollisionFlags(int flags) + { + m_collisionFlags = flags; + } + + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: + float getCcdSweptSphereRadius() const + { + return m_ccdSweptSphereRadius; + } + + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: + void setCcdSweptSphereRadius(float radius) + { + m_ccdSweptSphereRadius = radius; + } + + float getCcdSquareMotionThreshold() const + { + return m_ccdSquareMotionThreshold; + } + + + /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold + void setCcdSquareMotionThreshold(float ccdSquareMotionThreshold) + { + m_ccdSquareMotionThreshold = ccdSquareMotionThreshold; + } + + ///users can point to their objects, userPointer is not used by Bullet + void* getUserPointer() const + { + return m_userObjectPointer; + } + + ///users can point to their objects, userPointer is not used by Bullet + void setUserPointer(void* userPointer) + { + m_userObjectPointer = userPointer; + } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index e24a5c6a0b4..7edd3e8abfe 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -40,6 +40,7 @@ m_ownsBroadphasePairCache(false) { } + btCollisionWorld::btCollisionWorld() : m_dispatcher1(new btCollisionDispatcher()), m_broadphasePairCache(new btSimpleBroadphase()), @@ -60,7 +61,7 @@ btCollisionWorld::~btCollisionWorld() { btCollisionObject* collisionObject= (*i); - btBroadphaseProxy* bp = collisionObject->m_broadphaseHandle; + btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) { // @@ -98,21 +99,22 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho m_collisionObjects.push_back(collisionObject); //calculate new AABB - btTransform trans = collisionObject->m_worldTransform; + btTransform trans = collisionObject->getWorldTransform(); btVector3 minAabb; btVector3 maxAabb; - collisionObject->m_collisionShape->getAabb(trans,minAabb,maxAabb); + collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb); - int type = collisionObject->m_collisionShape->getShapeType(); - collisionObject->m_broadphaseHandle = getBroadphase()->createProxy( + int type = collisionObject->getCollisionShape()->getShapeType(); + collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( minAabb, maxAabb, type, collisionObject, collisionFilterGroup, collisionFilterMask - ); + )) ; + @@ -132,8 +134,8 @@ void btCollisionWorld::performDiscreteCollisionDetection() btVector3 aabbMin,aabbMax; for (size_t i=0;i<m_collisionObjects.size();i++) { - m_collisionObjects[i]->m_collisionShape->getAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax); - m_broadphasePairCache->setAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax); + m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax); + m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax); } m_broadphasePairCache->refreshOverlappingPairs(); @@ -155,7 +157,7 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) { - btBroadphaseProxy* bp = collisionObject->m_broadphaseHandle; + btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) { // @@ -163,7 +165,7 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) // getBroadphase()->cleanProxyFromPairs(bp); getBroadphase()->destroyProxy(bp); - collisionObject->m_broadphaseHandle = 0; + collisionObject->setBroadphaseHandle(0); } } @@ -318,15 +320,7 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r rayToTrans.setOrigin(rayToWorld); - //do culling based on aabb (rayFrom/rayTo) - btVector3 rayAabbMin = rayFromWorld; - btVector3 rayAabbMax = rayFromWorld; - rayAabbMin.setMin(rayToWorld); - rayAabbMax.setMax(rayToWorld); - - - /// brute force go over all objects. Once there is a broadphase, use that, or - /// add a raycast against aabb first. + /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD) std::vector<btCollisionObject*>::iterator iter; @@ -338,16 +332,16 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->m_collisionShape->getAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax); - - //check aabb overlap + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax)) + float hitLambda = 1.f; //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) { rayTestSingle(rayFromTrans,rayToTrans, collisionObject, - collisionObject->m_collisionShape, - collisionObject->m_worldTransform, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), resultCallback); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index a1cf3a0e5fd..e2b8eb11238 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -194,7 +194,7 @@ public: m_closestHitFraction = rayResult.m_hitFraction; m_collisionObject = rayResult.m_collisionObject; - m_hitNormalWorld = m_collisionObject->m_worldTransform.getBasis()*rayResult.m_hitNormalLocal; + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); return rayResult.m_hitFraction; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index febd726b556..43887d26a45 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -23,9 +23,9 @@ btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlg { btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; - assert (colObj->m_collisionShape->isCompound()); + assert (colObj->getCollisionShape()->isCompound()); - btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape); + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); int numChildren = compoundShape->getNumChildShapes(); int i; @@ -33,10 +33,10 @@ btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlg for (i=0;i<numChildren;i++) { btCollisionShape* childShape = compoundShape->getChildShape(i); - btCollisionShape* orgShape = colObj->m_collisionShape; - colObj->m_collisionShape = childShape; + btCollisionShape* orgShape = colObj->getCollisionShape(); + colObj->setCollisionShape( childShape ); m_childCollisionAlgorithms[i] = ci.m_dispatcher->findAlgorithm(colObj,otherObj); - colObj->m_collisionShape =orgShape; + colObj->setCollisionShape( orgShape ); } } @@ -56,8 +56,8 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; - assert (colObj->m_collisionShape->isCompound()); - btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape); + assert (colObj->getCollisionShape()->isCompound()); + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals @@ -74,18 +74,18 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt btCollisionShape* childShape = compoundShape->getChildShape(i); //backup - btTransform orgTrans = colObj->m_worldTransform; - btCollisionShape* orgShape = colObj->m_collisionShape; + btTransform orgTrans = colObj->getWorldTransform(); + btCollisionShape* orgShape = colObj->getCollisionShape(); btTransform childTrans = compoundShape->getChildTransform(i); btTransform newChildWorldTrans = orgTrans*childTrans ; - colObj->m_worldTransform = newChildWorldTrans; + colObj->setWorldTransform( newChildWorldTrans ); //the contactpoint is still projected back using the original inverted worldtrans - colObj->m_collisionShape = childShape; + colObj->setCollisionShape( childShape ); m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut); //revert back - colObj->m_collisionShape =orgShape; - colObj->m_worldTransform = orgTrans; + colObj->setCollisionShape( orgShape); + colObj->setWorldTransform( orgTrans ); } } @@ -95,9 +95,9 @@ float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* bod btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; - assert (colObj->m_collisionShape->isCompound()); + assert (colObj->getCollisionShape()->isCompound()); - btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape); + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals @@ -116,22 +116,22 @@ float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* bod btCollisionShape* childShape = compoundShape->getChildShape(i); //backup - btTransform orgTrans = colObj->m_worldTransform; - btCollisionShape* orgShape = colObj->m_collisionShape; + btTransform orgTrans = colObj->getWorldTransform(); + btCollisionShape* orgShape = colObj->getCollisionShape(); btTransform childTrans = compoundShape->getChildTransform(i); btTransform newChildWorldTrans = orgTrans*childTrans ; - colObj->m_worldTransform = newChildWorldTrans; + colObj->setWorldTransform( newChildWorldTrans ); - colObj->m_collisionShape = childShape; + colObj->setCollisionShape( childShape ); float frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); if (frac<hitFraction) { hitFraction = frac; } //revert back - colObj->m_collisionShape =orgShape; - colObj->m_worldTransform = orgTrans; + colObj->setCollisionShape( orgShape); + colObj->setWorldTransform( orgTrans); } return hitFraction; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 7cb0bba6206..8ad3967bac2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -89,7 +89,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) { btVector3 color(255,255,0); - btTransform& tr = ob->m_worldTransform; + btTransform& tr = ob->getWorldTransform(); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); @@ -105,14 +105,14 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); - if (m_convexBody->m_collisionShape->isConvex()) + if (m_convexBody->getCollisionShape()->isConvex()) { btTriangleShape tm(triangle[0],triangle[1],triangle[2]); tm.setMargin(m_collisionMarginTriangle); - btCollisionShape* tmpShape = ob->m_collisionShape; - ob->m_collisionShape = &tm; + btCollisionShape* tmpShape = ob->getCollisionShape(); + ob->setCollisionShape( &tm ); btCollisionAlgorithm* colAlgo = ci.m_dispatcher->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); @@ -124,7 +124,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); delete colAlgo; - ob->m_collisionShape = tmpShape; + ob->setCollisionShape( tmpShape ); } @@ -142,8 +142,8 @@ void btConvexTriangleCallback::setTimeStepAndCounters(float collisionMarginTrian //recalc aabbs btTransform convexInTriangleSpace; - convexInTriangleSpace = m_triBody->m_worldTransform.inverse() * m_convexBody->m_worldTransform; - btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->m_collisionShape); + convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform(); + btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape()); //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); float extraMargin = collisionMarginTriangle; @@ -167,14 +167,14 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* bod btCollisionObject* convexBody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; - if (triBody->m_collisionShape->isConcave()) + if (triBody->getCollisionShape()->isConcave()) { btCollisionObject* triOb = triBody; - ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->m_collisionShape); + ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->getCollisionShape()); - if (convexBody->m_collisionShape->isConvex()) + if (convexBody->getCollisionShape()->isConvex()) { float collisionMarginTriangle = concaveShape->getMargin(); @@ -207,8 +207,8 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject //only perform CCD above a certain threshold, this prevents blocking on the long run //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... - float squareMot0 = (convexbody->m_interpolationWorldTransform.getOrigin() - convexbody->m_worldTransform.getOrigin()).length2(); - if (squareMot0 < convexbody->m_ccdSquareMotionThreshold) + float squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); + if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) { return 1.f; } @@ -217,9 +217,9 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); //todo: only do if the motion exceeds the 'radius' - btTransform triInv = triBody->m_worldTransform.inverse(); - btTransform convexFromLocal = triInv * convexbody->m_worldTransform; - btTransform convexToLocal = triInv * convexbody->m_interpolationWorldTransform; + btTransform triInv = triBody->getWorldTransform().inverse(); + btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); + btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); struct LocalTriangleSphereCastCallback : public btTriangleCallback { @@ -270,24 +270,25 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject - if (triBody->m_collisionShape->isConcave()) + if (triBody->getCollisionShape()->isConcave()) { btVector3 rayAabbMin = convexFromLocal.getOrigin(); rayAabbMin.setMin(convexToLocal.getOrigin()); btVector3 rayAabbMax = convexFromLocal.getOrigin(); rayAabbMax.setMax(convexToLocal.getOrigin()); - rayAabbMin -= btVector3(convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius); - rayAabbMax += btVector3(convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius); + float ccdRadius0 = convexbody->getCcdSweptSphereRadius(); + rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); float curHitFraction = 1.f; //is this available? LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->m_ccdSweptSphereRadius,curHitFraction); + convexbody->getCcdSweptSphereRadius(),curHitFraction); - raycastCallback.m_hitFraction = convexbody->m_hitFraction; + raycastCallback.m_hitFraction = convexbody->getHitFraction(); btCollisionObject* concavebody = triBody; - ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->m_collisionShape; + ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->getCollisionShape(); if (triangleMesh) { @@ -296,9 +297,9 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject - if (raycastCallback.m_hitFraction < convexbody->m_hitFraction) + if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) { - convexbody->m_hitFraction = raycastCallback.m_hitFraction; + convexbody->setHitFraction( raycastCallback.m_hitFraction); return raycastCallback.m_hitFraction; } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 5347ef05bef..36b0d15eac5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -157,8 +157,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl checkPenetrationDepthSolver(); - btConvexShape* min0 = static_cast<btConvexShape*>(body0->m_collisionShape); - btConvexShape* min1 = static_cast<btConvexShape*>(body1->m_collisionShape); + btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape()); + btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape()); btGjkPairDetector::ClosestPointInput input; @@ -170,8 +170,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl // input.m_maximumDistanceSquared = 1e30f; - input.m_transformA = body0->m_worldTransform; - input.m_transformB = body1->m_worldTransform; + input.m_transformA = body0->getWorldTransform(); + input.m_transformB = body1->getWorldTransform(); resultOut->setPersistentManifold(m_manifoldPtr); m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); @@ -190,14 +190,13 @@ float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC float resultFraction = 1.f; - float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2(); + float squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); + float squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); - if (squareMot0 < col0->m_ccdSquareMotionThreshold && - squareMot0 < col0->m_ccdSquareMotionThreshold) + if (squareMot0 < col0->getCcdSquareMotionThreshold() && + squareMot1 < col1->getCcdSquareMotionThreshold()) return resultFraction; - - if (disableCcd) return 1.f; @@ -212,26 +211,26 @@ float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC /// Convex0 against sphere for Convex1 { - btConvexShape* convex0 = static_cast<btConvexShape*>(col0->m_collisionShape); + btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape()); - btSphereShape sphere1(col1->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform, - col1->m_worldTransform,col1->m_interpolationWorldTransform,result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), + col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) { //store result.m_fraction in both bodies - if (col0->m_hitFraction > result.m_fraction) - col0->m_hitFraction = result.m_fraction; + if (col0->getHitFraction()> result.m_fraction) + col0->setHitFraction( result.m_fraction ); - if (col1->m_hitFraction > result.m_fraction) - col1->m_hitFraction = result.m_fraction; + if (col1->getHitFraction() > result.m_fraction) + col1->setHitFraction( result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; @@ -245,26 +244,26 @@ float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC /// Sphere (for convex0) against Convex1 { - btConvexShape* convex1 = static_cast<btConvexShape*>(col1->m_collisionShape); + btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape()); - btSphereShape sphere0(col0->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform, - col1->m_worldTransform,col1->m_interpolationWorldTransform,result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), + col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) { //store result.m_fraction in both bodies - if (col0->m_hitFraction > result.m_fraction) - col0->m_hitFraction = result.m_fraction; + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction( result.m_fraction); - if (col1->m_hitFraction > result.m_fraction) - col1->m_hitFraction = result.m_fraction; + if (col1->getHitFraction() > result.m_fraction) + col1->setHitFraction( result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 1d3941101b2..cd22f3dd91e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -48,8 +48,8 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b m_body0(body0), m_body1(body1) { - m_rootTransA = body0->m_worldTransform; - m_rootTransB = body1->m_worldTransform; + m_rootTransA = body0->getWorldTransform(); + m_rootTransB = body1->getWorldTransform(); } @@ -81,8 +81,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b //User can override friction and/or restitution if (gContactAddedCallback && //and if either of the two bodies requires custom material - ((m_body0->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || - (m_body1->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) + ((m_body0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || + (m_body1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) { //experimental feature info, for per-triangle material etc. btCollisionObject* obj0 = isSwapped? m_body1 : m_body0; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index f9aae4e9c9a..3d70689e3eb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -17,10 +17,12 @@ subject to the following restrictions: #ifndef MANIFOLD_RESULT_H #define MANIFOLD_RESULT_H -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -struct btCollisionObject; +class btCollisionObject; class btPersistentManifold; class btManifoldPoint; + +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" + #include "LinearMath/btTransform.h" typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index d6ac86d19bd..be4f8686f30 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -42,8 +42,8 @@ void btSimulationIslandManager::findUnions(btDispatcher* dispatcher) ((colObj1) && ((colObj1)->mergesSimulationIslands()))) { - m_unionFind.unite((colObj0)->m_islandTag1, - (colObj1)->m_islandTag1); + m_unionFind.unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); } } } @@ -65,8 +65,8 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld { btCollisionObject* collisionObject= (*i); - collisionObject->m_islandTag1 = index; - collisionObject->m_hitFraction = 1.f; + collisionObject->setIslandTag(index); + collisionObject->setHitFraction(1.f); index++; } @@ -98,10 +98,10 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col if (collisionObject->mergesSimulationIslands()) { - collisionObject->m_islandTag1 = m_unionFind.find(index); + collisionObject->setIslandTag( m_unionFind.find(index) ); } else { - collisionObject->m_islandTag1 = -1; + collisionObject->setIslandTag(-1); } index++; } @@ -113,7 +113,7 @@ inline int getIslandId(const btPersistentManifold* lhs) int islandId; const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0()); const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1()); - islandId= rcolObj0->m_islandTag1>=0?rcolObj0->m_islandTag1:rcolObj1->m_islandTag1; + islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag(); return islandId; } @@ -158,19 +158,19 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; - if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1)) + if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { printf("error in island management\n"); } - assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1)); - if (colObj0->m_islandTag1 == islandId) + assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + if (colObj0->getIslandTag() == islandId) { - if (colObj0->GetActivationState()== ACTIVE_TAG) + if (colObj0->getActivationState()== ACTIVE_TAG) { allSleeping = false; } - if (colObj0->GetActivationState()== DISABLE_DEACTIVATION) + if (colObj0->getActivationState()== DISABLE_DEACTIVATION) { allSleeping = false; } @@ -184,16 +184,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, { int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; - if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1)) + if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { printf("error in island management\n"); } - assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1)); + assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - if (colObj0->m_islandTag1 == islandId) + if (colObj0->getIslandTag() == islandId) { - colObj0->SetActivationState( ISLAND_SLEEPING ); + colObj0->setActivationState( ISLAND_SLEEPING ); } } } else @@ -205,18 +205,18 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; - if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1)) + if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { printf("error in island management\n"); } - assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1)); + assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - if (colObj0->m_islandTag1 == islandId) + if (colObj0->getIslandTag() == islandId) { - if ( colObj0->GetActivationState() == ISLAND_SLEEPING) + if ( colObj0->getActivationState() == ISLAND_SLEEPING) { - colObj0->SetActivationState( WANTS_DEACTIVATION); + colObj0->setActivationState( WANTS_DEACTIVATION); } } } @@ -236,17 +236,17 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1()); //todo: check sleeping conditions! - if (((colObj0) && colObj0->GetActivationState() != ISLAND_SLEEPING) || - ((colObj1) && colObj1->GetActivationState() != ISLAND_SLEEPING)) + if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || + ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) { //kinematic objects don't merge islands, but wake up all connected objects - if (colObj0->isKinematicObject() && colObj0->GetActivationState() != ISLAND_SLEEPING) + if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) { - colObj1->SetActivationState(ACTIVE_TAG); + colObj1->setActivationState(ACTIVE_TAG); } - if (colObj1->isKinematicObject() && colObj1->GetActivationState() != ISLAND_SLEEPING) + if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) { - colObj0->SetActivationState(ACTIVE_TAG); + colObj0->setActivationState(ACTIVE_TAG); } //filtering for response diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index d32818444d7..1423c335407 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -57,11 +57,11 @@ void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,b btCollisionObject* boxObj = m_isSwapped? body0 : body1; - btSphereShape* sphere0 = (btSphereShape*)sphereObj ->m_collisionShape; + btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape(); btVector3 normalOnSurfaceB; btVector3 pOnBox,pOnSphere; - btVector3 sphereCenter = sphereObj->m_worldTransform.getOrigin(); + btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin(); btScalar radius = sphere0->getRadius(); float dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius); @@ -93,14 +93,14 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box btScalar margins; btVector3 bounds[2]; - btBoxShape* boxShape= (btBoxShape*)boxObj->m_collisionShape; + btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape(); bounds[0] = -boxShape->getHalfExtents(); bounds[1] = boxShape->getHalfExtents(); margins = boxShape->getMargin();//also add sphereShape margin? - const btTransform& m44T = boxObj->m_worldTransform; + const btTransform& m44T = boxObj->getWorldTransform(); btVector3 boundsVec[2]; btScalar fPenetration; @@ -209,7 +209,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* n[4].setValue( 0.0f, 1.0f, 0.0f ); n[5].setValue( 0.0f, 0.0f, 1.0f ); - const btTransform& m44T = boxObj->m_worldTransform; + const btTransform& m44T = boxObj->getWorldTransform(); // convert point in local space prel = m44T.invXform( sphereCenter); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index bb04833ca5a..eb05bf974c5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -44,10 +44,10 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 if (!m_manifoldPtr) return; - btSphereShape* sphere0 = (btSphereShape*)col0->m_collisionShape; - btSphereShape* sphere1 = (btSphereShape*)col1->m_collisionShape; + btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape(); + btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape(); - btVector3 diff = col0->m_worldTransform.getOrigin()- col1->m_worldTransform.getOrigin(); + btVector3 diff = col0->getWorldTransform().getOrigin()- col1->getWorldTransform().getOrigin(); float len = diff.length(); btScalar radius0 = sphere0->getRadius(); btScalar radius1 = sphere1->getRadius(); @@ -61,9 +61,9 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 btVector3 normalOnSurfaceB = diff / len; ///point on A (worldspace) - btVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB; + btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) - btVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB; + btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index ca589ef8629..6f74c488659 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -48,8 +48,8 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co if (!m_manifoldPtr) return; - btSphereShape* sphere = (btSphereShape*)col0->m_collisionShape; - btTriangleShape* triangle = (btTriangleShape*)col1->m_collisionShape; + btSphereShape* sphere = (btSphereShape*)col0->getCollisionShape(); + btTriangleShape* triangle = (btTriangleShape*)col1->getCollisionShape(); /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); @@ -57,8 +57,8 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co btDiscreteCollisionDetectorInterface::ClosestPointInput input; input.m_maximumDistanceSquared = 1e30f;//todo: tighter bounds - input.m_transformA = col0->m_worldTransform; - input.m_transformB = col1->m_worldTransform; + input.m_transformA = col0->getWorldTransform(); + input.m_transformB = col1->getWorldTransform(); detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h index b137eb1150e..e85a36c0b73 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -255,6 +255,37 @@ public: return "Box"; } + virtual int getNumPreferredPenetrationDirections() const + { + return 6; + } + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + { + switch (index) + { + case 0: + penetrationVector.setValue(1.f,0.f,0.f); + break; + case 1: + penetrationVector.setValue(-1.f,0.f,0.f); + break; + case 2: + penetrationVector.setValue(0.f,1.f,0.f); + break; + case 3: + penetrationVector.setValue(0.f,-1.f,0.f); + break; + case 4: + penetrationVector.setValue(0.f,0.f,1.f); + break; + case 5: + penetrationVector.setValue(0.f,0.f,-1.f); + break; + default: + assert(0); + } + } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h index 3ffde1ba5ed..0de334b3521 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -26,7 +26,7 @@ subject to the following restrictions: //todo: get rid of this btConvexCastResult thing! struct btConvexCastResult; - +#define MAX_PREFERRED_PENETRATION_DIRECTIONS 10 /// btConvexShape is an abstract shape interface. /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. @@ -84,6 +84,17 @@ public: return m_collisionMargin; } + virtual int getNumPreferredPenetrationDirections() const + { + return 0; + } + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + { + assert(0); + } + + }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index aaadb82eb4b..fde4d95da14 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -20,17 +20,17 @@ subject to the following restrictions: btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres) :m_inertiaHalfExtents(inertiaHalfExtents) { - m_minRadius = 1e30f; + float startMargin = 1e30f; m_numSpheres = numSpheres; for (int i=0;i<m_numSpheres;i++) { m_localPositions[i] = positions[i]; m_radi[i] = radi[i]; - if (radi[i] < m_minRadius) - m_minRadius = radi[i]; + if (radi[i] < startMargin) + startMargin = radi[i]; } - setMargin(m_minRadius); + setMargin(startMargin); } @@ -64,7 +64,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons for (i=0;i<m_numSpheres;i++) { - vtx = (*pos) +vec*((*rad)-m_minRadius); + vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin(); pos++; rad++; newDot = vec.dot(vtx); @@ -96,7 +96,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons for (int i=0;i<m_numSpheres;i++) { - vtx = (*pos) +vec*((*rad)-m_minRadius); + vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin(); pos++; rad++; newDot = vec.dot(vtx); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index 6a9151df281..2dde9d3855a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -31,9 +31,7 @@ class btMultiSphereShape : public btConvexShape btVector3 m_inertiaHalfExtents; int m_numSpheres; - float m_minRadius; - - + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp new file mode 100644 index 00000000000..078ba38fdf4 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp @@ -0,0 +1,41 @@ +/* +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 "btTriangleBuffer.h" + + +///example usage of this class: +// btTriangleBuffer triBuf; +// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax); +// for (int i=0;i<triBuf.getNumTriangles();i++) +// { +// const btTriangle& tri = triBuf.getTriangle(i); +// //do something useful here with the triangle +// } + + + + +void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex) +{ + btTriangle tri; + tri.m_vertex0 = triangle[0]; + tri.m_vertex1 = triangle[1]; + tri.m_vertex2 = triangle[2]; + tri.m_partId = partId; + tri.m_triangleIndex = triangleIndex; + + m_triangleBuffer.push_back(tri); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h new file mode 100644 index 00000000000..4f421a616c8 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -0,0 +1,60 @@ +/* +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_TRIANGLE_BUFFER_H +#define BT_TRIANGLE_BUFFER_H + +#include "btTriangleCallback.h" +#include <vector> + +struct btTriangle +{ + btVector3 m_vertex0; + btVector3 m_vertex1; + btVector3 m_vertex2; + int m_partId; + int m_triangleIndex; +}; + +///btTriangleBuffer can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles' +class btTriangleBuffer : public btTriangleCallback +{ + + std::vector<btTriangle> m_triangleBuffer; + +public: + + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); + + int getNumTriangles() const + { + return int(m_triangleBuffer.size()); + } + + const btTriangle& getTriangle(int index) const + { + return m_triangleBuffer[index]; + } + + void clearBuffer() + { + m_triangleBuffer.clear(); + } + +}; + + +#endif //BT_TRIANGLE_BUFFER_H
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index 690d1e849de..171dcf33b15 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -46,6 +46,15 @@ class btTriangleMesh : public btStridingMeshInterface m_triangles.push_back(tri); } + int getNumTriangles() const + { + return m_triangles.size(); + } + + const btMyTriangle& getTriangle(int index) const + { + return m_triangles[index]; + } //StridingMeshInterface interface implementation diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h index 8f0a06f7586..8e6e2daa4fb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -157,6 +157,18 @@ public: return "Triangle"; } + virtual int getNumPreferredPenetrationDirections() const + { + return 2; + } + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + { + calcNormal(penetrationVector); + if (index) + penetrationVector *= -1.f; + } + }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index 8889699b395..ef2b480fa62 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef DISCRETE_COLLISION_DETECTOR_INTERFACE_H -#define DISCRETE_COLLISION_DETECTOR_INTERFACE_H +#ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H +#define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" @@ -84,4 +84,4 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result } }; -#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE_H +#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE1_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 4b38ced7f12..507fcaf21f7 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -22,12 +22,13 @@ subject to the following restrictions: #include <stdio.h> //for debug printf #endif -static const btScalar rel_error = btScalar(1.0e-5); -btScalar rel_error2 = rel_error * rel_error; -float maxdist2 = 1.e30f; +//must be above the machine epsilon +#define REL_ERROR2 1.0e-6f + #ifdef __SPU__ #include <spu_printf.h> +#define printf spu_printf #endif //__SPU__ btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) @@ -36,7 +37,9 @@ m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), m_minkowskiB(objectB), -m_ignoreMargin(false) +m_ignoreMargin(false), +m_lastUsedMethod(-1), +m_catchDegeneracies(0) { } @@ -45,6 +48,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btScalar distance=0.f; btVector3 normalInB(0.f,0.f,0.f); btVector3 pointOnA,pointOnB; + btTransform localTransA = input.m_transformA; + btTransform localTransB = input.m_transformB; + btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * 0.5f; + localTransA.getOrigin() -= positionOffset; + localTransB.getOrigin() -= positionOffset; float marginA = m_minkowskiA->getMargin(); float marginB = m_minkowskiB->getMargin(); @@ -56,12 +64,16 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& marginB = 0.f; } - int curIter = 0; + m_curIter = 0; int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? + m_cachedSeparatingAxis.setValue(0,1,0); bool isValid = false; bool checkSimplex = false; bool checkPenetration = true; + m_degenerateSimplex = false; + + m_lastUsedMethod = -1; { btScalar squaredDistance = SIMD_INFINITY; @@ -81,8 +93,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); - btPoint3 pWorld = input.m_transformA(pInA); - btPoint3 qWorld = input.m_transformB(qInB); + btPoint3 pWorld = localTransA(pInA); + btPoint3 qWorld = localTransB(qInB); btVector3 w = pWorld - qWorld; delta = m_cachedSeparatingAxis.dot(w); @@ -101,7 +113,16 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& break; } // are we getting any closer ? - if (squaredDistance - delta <= squaredDistance * rel_error2) + float f0 = squaredDistance - delta; + float f1 = squaredDistance * REL_ERROR2; + + if (f0 <= 0.f) + { + m_degenerateSimplex = 2; + } + + if (f0 >= 0.f && (f0 <= f1)) + //if (f0 <= f1) { checkSimplex = true; break; @@ -112,6 +133,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& //calculate the closest point to the origin (update vector v) if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) { + m_degenerateSimplex = 1; checkSimplex = true; break; } @@ -130,10 +152,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& } //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (curIter++ > gGjkMaxIter) + if (m_curIter++ > gGjkMaxIter) { #if defined(DEBUG) || defined (_DEBUG) - printf("btGjkPairDetector maxIter exceeded:%i\n",curIter); + + printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", m_cachedSeparatingAxis.getX(), m_cachedSeparatingAxis.getY(), @@ -141,6 +164,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& squaredDistance, m_minkowskiA->getShapeType(), m_minkowskiB->getShapeType()); + #endif break; @@ -164,20 +188,27 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& normalInB = pointOnA-pointOnB; float lenSqr = m_cachedSeparatingAxis.length2(); //valid normal - if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) + //if (lenSqr > (0.1f*margin)) //SIMD_EPSILON*SIMD_EPSILON)) + if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) { float rlen = 1.f / btSqrt(lenSqr ); normalInB *= rlen; //normalize btScalar s = btSqrt(squaredDistance); + ASSERT(s > btScalar(0.0)); pointOnA -= m_cachedSeparatingAxis * (marginA / s); pointOnB += m_cachedSeparatingAxis * (marginB / s); distance = ((1.f/rlen) - margin); isValid = true; + m_lastUsedMethod = 1; + } else + { + m_lastUsedMethod = 2; } } - if (checkPenetration && !isValid) + //if (checkPenetration && !isValid) + if (checkPenetration && (!isValid || (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex) )) { //penetration case @@ -185,27 +216,46 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (m_penetrationDepthSolver) { // Penetration depth case. - isValid = m_penetrationDepthSolver->calcPenDepth( + btVector3 tmpPointOnA,tmpPointOnB; + bool isValid2 = m_penetrationDepthSolver->calcPenDepth( *m_simplexSolver, m_minkowskiA,m_minkowskiB, - input.m_transformA,input.m_transformB, - m_cachedSeparatingAxis, pointOnA, pointOnB, + localTransA,localTransB, + m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, debugDraw ); - if (isValid) + if (isValid2) { - normalInB = pointOnB-pointOnA; - float lenSqr = normalInB.length2(); + btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; + float lenSqr = tmpNormalInB.length2(); if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { - normalInB /= btSqrt(lenSqr); - distance = -(pointOnA-pointOnB).length(); + tmpNormalInB /= btSqrt(lenSqr); + float distance2 = -(tmpPointOnA-tmpPointOnB).length(); + //only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + isValid = true; + m_lastUsedMethod = 3; + } else + { + + } } else { - isValid = false; + //isValid = false; + m_lastUsedMethod = 4; } + } else + { + m_lastUsedMethod = 5; } + } } } @@ -219,7 +269,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output.addContactPoint( normalInB, - pointOnB, + pointOnB+positionOffset, distance); //printf("gjk add:%f",distance); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index c4842cd3023..09c1669bd78 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -43,6 +43,12 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface public: + //some debugging to fix degeneracy problems + int m_lastUsedMethod; + int m_curIter; + int m_degenerateSimplex; + int m_catchDegeneracies; + btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); virtual ~btGjkPairDetector() {}; @@ -68,11 +74,13 @@ public: m_penetrationDepthSolver = penetrationDepthSolver; } + ///don't use setIgnoreMargin, it's for Bullet's internal use void setIgnoreMargin(bool ignoreMargin) { m_ignoreMargin = ignoreMargin; } + }; #endif //GJK_PAIR_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index 34daacf26ac..74cf2b7c071 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -20,32 +20,10 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -struct MyResult : public btDiscreteCollisionDetectorInterface::Result -{ - MyResult():m_hasResult(false) - { - } - - btVector3 m_normalOnBInWorld; - btVector3 m_pointInWorld; - float m_depth; - bool m_hasResult; - - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) - { - } - void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) - { - m_normalOnBInWorld = normalOnBInWorld; - m_pointInWorld = pointInWorld; - m_depth = depth; - m_hasResult = true; - } -}; #define NUM_UNITSPHERE_POINTS 42 -static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS] = +static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = { btVector3(0.000000f , -0.000000f,-1.000000f), btVector3(0.723608f , -0.525725f,-0.447219f), @@ -100,6 +78,31 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s ) { + + struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result + { + + btIntermediateResult():m_hasResult(false) + { + } + + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + float m_depth; + bool m_hasResult; + + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + } + void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + { + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; + m_hasResult = true; + } + }; + //just take fixed number of orientation, and sample the penetration depth in that direction float minProj = 1e30f; btVector3 minNorm; @@ -110,22 +113,62 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s #define USE_BATCHED_SUPPORT 1 #ifdef USE_BATCHED_SUPPORT - btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS]; - btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS]; - btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS]; - btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS]; + + btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; int i; - for (i=0;i<NUM_UNITSPHERE_POINTS;i++) + int numSampleDirections = NUM_UNITSPHERE_POINTS; + + for (i=0;i<numSampleDirections;i++) { const btVector3& norm = sPenetrationDirections[i]; - seperatingAxisInABatch[i] = (-norm)* transA.getBasis(); - seperatingAxisInBBatch[i] = norm * transB.getBasis(); + seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ; + seperatingAxisInBBatch[i] = norm * transB.getBasis() ; } - convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,NUM_UNITSPHERE_POINTS); - convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS); - for (i=0;i<NUM_UNITSPHERE_POINTS;i++) + { + int numPDA = convexA->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int i=0;i<numPDA;i++) + { + btVector3 norm; + convexA->getPreferredPenetrationDirection(i,norm); + norm = transA.getBasis() * norm; + sPenetrationDirections[numSampleDirections] = norm; + seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); + seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); + numSampleDirections++; + } + } + } + + { + int numPDB = convexB->getNumPreferredPenetrationDirections(); + if (numPDB) + { + for (int i=0;i<numPDB;i++) + { + btVector3 norm; + convexB->getPreferredPenetrationDirection(i,norm); + norm = transB.getBasis() * norm; + sPenetrationDirections[numSampleDirections] = norm; + seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); + seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); + numSampleDirections++; + } + } + } + + + + convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); + convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); + + for (i=0;i<numSampleDirections;i++) { const btVector3& norm = sPenetrationDirections[i]; seperatingAxisInA = seperatingAxisInABatch[i]; @@ -148,7 +191,40 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s } } #else - for (int i=0;i<NUM_UNITSPHERE_POINTS;i++) + + int numSampleDirections = NUM_UNITSPHERE_POINTS; + + { + int numPDA = convexA->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int i=0;i<numPDA;i++) + { + btVector3 norm; + convexA->getPreferredPenetrationDirection(i,norm); + norm = transA.getBasis() * norm; + sPenetrationDirections[numSampleDirections] = norm; + numSampleDirections++; + } + } + } + + { + int numPDB = convexB->getNumPreferredPenetrationDirections(); + if (numPDB) + { + for (int i=0;i<numPDB;i++) + { + btVector3 norm; + convexB->getPreferredPenetrationDirection(i,norm); + norm = transB.getBasis() * norm; + sPenetrationDirections[numSampleDirections] = norm; + numSampleDirections++; + } + } + } + + for (int i=0;i<numSampleDirections;i++) { const btVector3& norm = sPenetrationDirections[i]; seperatingAxisInA = (-norm)* transA.getBasis(); @@ -174,10 +250,15 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minA += minNorm*convexA->getMargin(); minB -= minNorm*convexB->getMargin(); + //no penetration + if (minProj < 0.f) + return false; + minProj += (convexA->getMargin() + convexB->getMargin()); - + + //#define DEBUG_DRAW 1 #ifdef DEBUG_DRAW @@ -213,7 +294,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_transformB = transB; input.m_maximumDistanceSquared = 1e30f;//minProj; - MyResult res; + btIntermediateResult res; gjkdet.getClosestPoints(input,res,debugDraw); float correctedMinNorm = minProj - res.m_depth; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 429ad54d517..8d22689ec53 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -164,7 +164,8 @@ float resolveSingleFriction( float combinedFriction = cpd->m_friction; btScalar limit = cpd->m_appliedImpulse * combinedFriction; - //if (contactPoint.m_appliedImpulse>0.f) + + if (cpd->m_appliedImpulse>0.f) //friction { //apply friction in the 2 tangential directions @@ -182,11 +183,12 @@ float resolveSingleFriction( // calculate j that moves us to zero relative velocity j1 = -vrel * cpd->m_jacDiagABInvTangent0; - float total = cpd->m_accumulatedTangentImpulse0 + j1; - GEN_set_min(total, limit); - GEN_set_max(total, -limit); - j1 = total - cpd->m_accumulatedTangentImpulse0; - cpd->m_accumulatedTangentImpulse0 = total; + float oldTangentImpulse = cpd->m_accumulatedTangentImpulse0; + cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1; + GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit); + GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit); + j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse; + } { // 2nd tangent @@ -195,11 +197,11 @@ float resolveSingleFriction( // calculate j that moves us to zero relative velocity j2 = -vrel * cpd->m_jacDiagABInvTangent1; - float total = cpd->m_accumulatedTangentImpulse1 + j2; - GEN_set_min(total, limit); - GEN_set_max(total, -limit); - j2 = total - cpd->m_accumulatedTangentImpulse1; - cpd->m_accumulatedTangentImpulse1 = total; + float oldTangentImpulse = cpd->m_accumulatedTangentImpulse1; + cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2; + GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit); + GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit); + j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse; } #ifdef USE_INTERNAL_APPLY_IMPULSE @@ -395,4 +397,5 @@ float resolveSingleFrictionEmpty( const btContactSolverInfo& solverInfo) { return 0.f; -};
\ No newline at end of file +}; + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index a8ab1bbad3a..b5783f824d4 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -21,6 +21,7 @@ subject to the following restrictions: static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f }; static const int kAxisA[] = { 1, 0, 0 }; static const int kAxisB[] = { 2, 2, 1 }; +#define GENERIC_D6_DISABLE_WARMSTARTING 1 btGeneric6DofConstraint::btGeneric6DofConstraint() { @@ -47,7 +48,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& void btGeneric6DofConstraint::buildJacobian() { - btVector3 normal(0,0,0); + btVector3 localNormalInA(0,0,0); const btVector3& pivotInA = m_frameInA.getOrigin(); const btVector3& pivotInB = m_frameInB.getOrigin(); @@ -64,7 +65,9 @@ void btGeneric6DofConstraint::buildJacobian() { if (isLimited(i)) { - normal[i] = 1; + localNormalInA[i] = 1; + btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA; + // Create linear atom new (&m_jacLinear[i]) btJacobianEntry( @@ -72,19 +75,24 @@ void btGeneric6DofConstraint::buildJacobian() m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(), m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(), - normal, + normalWorld, m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); + //optionally disable warmstarting +#ifdef GENERIC_D6_DISABLE_WARMSTARTING + m_accumulatedImpulse[i] = 0.f; +#endif //GENERIC_D6_DISABLE_WARMSTARTING + // Apply accumulated impulse - btVector3 impulse_vector = m_accumulatedImpulse[i] * normal; + btVector3 impulse_vector = m_accumulatedImpulse[i] * normalWorld; m_rbA.applyImpulse( impulse_vector, rel_pos1); m_rbB.applyImpulse(-impulse_vector, rel_pos2); - normal[i] = 0; + localNormalInA[i] = 0; } } @@ -106,6 +114,10 @@ void btGeneric6DofConstraint::buildJacobian() m_rbA.getInvInertiaDiagLocal(), m_rbB.getInvInertiaDiagLocal()); +#ifdef GENERIC_D6_DISABLE_WARMSTARTING + m_accumulatedImpulse[i + 3] = 0.f; +#endif //GENERIC_D6_DISABLE_WARMSTARTING + // Apply accumulated impulse btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis; @@ -115,6 +127,52 @@ void btGeneric6DofConstraint::buildJacobian() } } +float getMatrixElem(const btMatrix3x3& mat,int index) +{ + int row = index%3; + int col = index / 3; + return mat[row][col]; +} + +///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html +bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz -cy*sz sy + // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + +/// 0..8 + + if (getMatrixElem(mat,2) < 1.0f) + { + if (getMatrixElem(mat,2) > -1.0f) + { + xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8)); + xyz[1] = btAsin(getMatrixElem(mat,2)); + xyz[2] = btAtan2(-getMatrixElem(mat,1),getMatrixElem(mat,0)); + return true; + } + else + { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = 0.0f; + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); + xyz[1] = SIMD_HALF_PI; + xyz[2] = 0.0; + return false; + } + return false; +} + + void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) { btScalar tau = 0.1f; @@ -126,7 +184,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - btVector3 normal(0,0,0); + btVector3 localNormalInA(0,0,0); int i; // linear @@ -137,8 +195,10 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + localNormalInA.setValue(0,0,0); + localNormalInA[i] = 1; + btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA; - normal[i] = 1; btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal(); //velocity error (first order error) @@ -146,19 +206,59 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) m_rbB.getLinearVelocity(),angvelB); //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); - - btScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; - m_accumulatedImpulse[i] += impulse; + btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); + btScalar lo = -1e30f; + btScalar hi = 1e30f; + + //handle the limits + if (m_lowerLimit[i] < m_upperLimit[i]) + { + { + if (depth > m_upperLimit[i]) + { + depth -= m_upperLimit[i]; + lo = 0.f; + + } else + { + if (depth < m_lowerLimit[i]) + { + depth -= m_lowerLimit[i]; + hi = 0.f; + } else + { + continue; + } + } + } + } - btVector3 impulse_vector = normal * impulse; + btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; + float oldNormalImpulse = m_accumulatedImpulse[i]; + float sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[i] = sum > hi ? 0.f : sum < lo ? 0.f : sum; + normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse; + + btVector3 impulse_vector = normalWorld * normalImpulse; m_rbA.applyImpulse( impulse_vector, rel_pos1); m_rbB.applyImpulse(-impulse_vector, rel_pos2); - normal[i] = 0; + localNormalInA[i] = 0; } } + btVector3 axis; + btScalar angle; + btTransform frameAWorld = m_rbA.getCenterOfMassTransform() * m_frameInA; + btTransform frameBWorld = m_rbB.getCenterOfMassTransform() * m_frameInB; + + btTransformUtil::calculateDiffAxisAngle(frameAWorld,frameBWorld,axis,angle); + btQuaternion diff(axis,angle); + btMatrix3x3 diffMat (diff); + btVector3 xyz; + ///this is not perfect, we can first check which axis are limited, and choose a more appropriate order + MatrixToEulerXYZ(diffMat,xyz); + // angular for (i=0;i<3;i++) { @@ -179,13 +279,46 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btScalar rel_pos = kSign[i] * axisA.dot(axisB); + btScalar lo = -1e30f; + btScalar hi = 1e30f; + + //handle the twist limit + if (m_lowerLimit[i+3] < m_upperLimit[i+3]) + { + //clamp the values + btScalar loLimit = m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f; + btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f; + + float projAngle = -2.*xyz[i]; + + if (projAngle < loLimit) + { + hi = 0.f; + rel_pos = (loLimit - projAngle); + } else + { + if (projAngle > hiLimit) + { + lo = 0.f; + rel_pos = (hiLimit - projAngle); + } else + { + continue; + } + } + } + //impulse - btScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; - m_accumulatedImpulse[i + 3] += impulse; + + btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; + float oldNormalImpulse = m_accumulatedImpulse[i+3]; + float sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[i+3] = sum > hi ? 0.f : sum < lo ? 0.f : sum; + normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse; // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) btVector3 axis = kSign[i] * axisA.cross(axisB); - btVector3 impulse_vector = axis * impulse; + btVector3 impulse_vector = axis * normalImpulse; m_rbA.applyTorqueImpulse( impulse_vector); m_rbB.applyTorqueImpulse(-impulse_vector); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index ac06f718c9e..568bcc40939 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -32,6 +32,27 @@ int totalCpd = 0; int gTotalContactPoints = 0; +struct btOrderIndex +{ + short int m_manifoldIndex; + short int m_pointIndex; +}; + +#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384 +static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS]; +static unsigned long btSeed2 = 0; +unsigned long btRand2() +{ + btSeed2 = (1664525L*btSeed2 + 1013904223L) & 0xffffffff; + return btSeed2; +} + +int btRandInt2 (int n) +{ + float a = float(n) / 4294967296.0f; + return (int) (float(btRand2()) * a); +} + bool MyContactDestroyedCallback(void* userPersistentData) { assert (userPersistentData); @@ -69,52 +90,62 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man btProfiler::beginBlock("solve"); #endif //USE_PROFILE + int totalPoints = 0; + { int j; for (j=0;j<numManifolds;j++) { - int k=j; - prepareConstraints(manifoldPtr[k],info,debugDrawer); - solve(manifoldPtr[k],info,0,debugDrawer); + prepareConstraints(manifoldPtr[j],info,debugDrawer); } } - - - //should traverse the contacts random order... - int i; - for ( i = 0;i<numiter-1;i++) + { int j; for (j=0;j<numManifolds;j++) { - int k=j; - if (i&1) - k=numManifolds-j-1; - - solve(manifoldPtr[k],info,i,debugDrawer); + for (int p=0;p<manifoldPtr[j]->getNumContacts();p++) + { + gOrder[totalPoints].m_manifoldIndex = j; + gOrder[totalPoints].m_pointIndex = p; + totalPoints++; + } } - } -#ifdef USE_PROFILE - btProfiler::endBlock("solve"); - - btProfiler::beginBlock("solveFriction"); -#endif //USE_PROFILE - - //now solve the friction - for (i = 0;i<numiter-1;i++) + + + //should traverse the contacts random order... + int iteration; + for ( iteration = 0;iteration<numiter-1;iteration++) { int j; - for (j=0;j<numManifolds;j++) + if ((iteration & 7) == 0) { + for (j=0; j<totalPoints; ++j) { + btOrderIndex tmp = gOrder[j]; + int swapi = btRandInt2(j+1); + gOrder[j] = gOrder[swapi]; + gOrder[swapi] = tmp; + } + } + + for (j=0;j<totalPoints;j++) { - int k = j; - if (i&1) - k=numManifolds-j-1; - solveFriction(manifoldPtr[k],info,i,debugDrawer); + btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; + solve( (btRigidBody*)manifold->getBody0(), + (btRigidBody*)manifold->getBody1() + ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + } + + for (j=0;j<totalPoints;j++) + { + btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; + solveFriction((btRigidBody*)manifold->getBody0(), + (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); } } + #ifdef USE_PROFILE - btProfiler::endBlock("solveFriction"); + btProfiler::endBlock("solve"); #endif //USE_PROFILE return 0.f; @@ -225,7 +256,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol btScalar penVel = -cpd->m_penetration/info.m_timeStep; - if (cpd->m_restitution >= penVel) + if (cpd->m_restitution > penVel) { cpd->m_penetration = 0.f; } @@ -233,7 +264,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol float relaxation = info.m_damping; - cpd->m_appliedImpulse *= relaxation; + cpd->m_appliedImpulse =0.f;//*= relaxation; //for friction cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse; @@ -260,8 +291,8 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol btVector3 totalImpulse = #ifndef NO_FRICTION_WARMSTART - cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+ - cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+ + cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+ + cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+ #endif //NO_FRICTION_WARMSTART cp.m_normalWorldOnB*cpd->m_appliedImpulse; @@ -304,29 +335,16 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol } } -float btSequentialImpulseConstraintSolver::solve(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) { - btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); - btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); - float maxImpulse = 0.f; { - const int numpoints = manifoldPtr->getNumContacts(); btVector3 color(0,1,0); - for (int i=0;i<numpoints ;i++) { - - int j=i; - if (iter % 2) - j = numpoints-1-i; - else - j=i; - - btManifoldPoint& cp = manifoldPtr->getContactPoint(j); - if (cp.getDistance() <= 0.f) + if (cp.getDistance() <= 0.f) { if (iter == 0) @@ -353,24 +371,15 @@ float btSequentialImpulseConstraintSolver::solve(btPersistentManifold* manifoldP return maxImpulse; } -float btSequentialImpulseConstraintSolver::solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +float btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) { - btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); - btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); { - const int numpoints = manifoldPtr->getNumContacts(); btVector3 color(0,1,0); - for (int i=0;i<numpoints ;i++) { - - int j=i; - //if (iter % 2) - // j = numpoints-1-i; - - btManifoldPoint& cp = manifoldPtr->getContactPoint(j); + if (cp.getDistance() <= 0.f) { diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index e399a5cd734..8264f5071dd 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -29,8 +29,9 @@ class btIDebugDraw; /// Applies impulses for combined restitution and penetration recovery and to simulate friction class btSequentialImpulseConstraintSolver : public btConstraintSolver { - float solve(btPersistentManifold* manifold, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - float solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + + float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index ea7d5c8b903..cb3fa72d440 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -17,7 +17,7 @@ subject to the following restrictions: #include "btTypedConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" -static btRigidBody s_fixed(0, btTransform::getIdentity(),0); +static btRigidBody s_fixed(0, 0,0); btTypedConstraint::btTypedConstraint() : m_userConstraintType(-1), diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 1bfc9b33348..125e933ad90 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -56,9 +56,10 @@ subject to the following restrictions: #include <algorithm> -btDiscreteDynamicsWorld::btDiscreteDynamicsWorld() + +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btConstraintSolver* constraintSolver) :btDynamicsWorld(), -m_constraintSolver(new btSequentialImpulseConstraintSolver), +m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver), m_debugDrawer(0), m_gravity(0,-10,0), m_localTime(1.f/60.f), @@ -66,10 +67,10 @@ m_profileTimings(0) { m_islandManager = new btSimulationIslandManager(); m_ownsIslandManager = true; - m_ownsConstraintSolver = true; - + m_ownsConstraintSolver = (constraintSolver==0); } + btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) :btDynamicsWorld(dispatcher,pairCache), m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver), @@ -103,7 +104,7 @@ void btDiscreteDynamicsWorld::saveKinematicState(float timeStep) if (body) { btTransform predictedTrans; - if (body->GetActivationState() != ISLAND_SLEEPING) + if (body->getActivationState() != ISLAND_SLEEPING) { if (body->isKinematicObject()) { @@ -117,42 +118,82 @@ void btDiscreteDynamicsWorld::saveKinematicState(float timeStep) void btDiscreteDynamicsWorld::synchronizeMotionStates() { - //todo: iterate over awake simulation islands! - for (unsigned int i=0;i<m_collisionObjects.size();i++) + //debug vehicle wheels + + { - btCollisionObject* colObj = m_collisionObjects[i]; - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + //todo: iterate over awake simulation islands! + for (unsigned int i=0;i<m_collisionObjects.size();i++) { - btVector3 color(255.f,255.f,255.f); - switch(colObj->GetActivationState()) + btCollisionObject* colObj = m_collisionObjects[i]; + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) { - case ACTIVE_TAG: - color = btVector3(255.f,255.f,255.f); break; - case ISLAND_SLEEPING: - color = btVector3(0.f,255.f,0.f);break; - case WANTS_DEACTIVATION: - color = btVector3(0.f,255.f,255.f);break; - case DISABLE_DEACTIVATION: - color = btVector3(255.f,0.f,0.f);break; - case DISABLE_SIMULATION: - color = btVector3(255.f,255.f,0.f);break; - default: + btVector3 color(255.f,255.f,255.f); + switch(colObj->getActivationState()) { - color = btVector3(255.f,0.f,0.f); - } - }; + case ACTIVE_TAG: + color = btVector3(255.f,255.f,255.f); break; + case ISLAND_SLEEPING: + color = btVector3(0.f,255.f,0.f);break; + case WANTS_DEACTIVATION: + color = btVector3(0.f,255.f,255.f);break; + case DISABLE_DEACTIVATION: + color = btVector3(255.f,0.f,0.f);break; + case DISABLE_SIMULATION: + color = btVector3(255.f,255.f,0.f);break; + default: + { + color = btVector3(255.f,0.f,0.f); + } + }; - debugDrawObject(colObj->m_worldTransform,colObj->m_collisionShape,color); + debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); + } + btRigidBody* body = btRigidBody::upcast(colObj); + if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) + { + if (body->getActivationState() != ISLAND_SLEEPING) + { + btTransform interpolatedTransform; + btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform); + body->getMotionState()->setWorldTransform(interpolatedTransform); + } + } } - btRigidBody* body = btRigidBody::upcast(colObj); - if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) + } + + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + for (unsigned int i=0;i<this->m_vehicles.size();i++) { - if (body->GetActivationState() != ISLAND_SLEEPING) + for (int v=0;v<m_vehicles[i]->getNumWheels();v++) { - btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->m_interpolationWorldTransform, - body->m_interpolationLinearVelocity,body->m_interpolationAngularVelocity,m_localTime,interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); + 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); + } } } @@ -178,8 +219,15 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo //variable timestep fixedTimeStep = timeStep; m_localTime = timeStep; - numSimulationSubSteps = 1; - maxSubSteps = 1; + if (btFuzzyZero(timeStep)) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } } //process some debugging flags @@ -187,7 +235,7 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo { gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; } - if (!btFuzzyZero(timeStep) && numSimulationSubSteps) + if (numSimulationSubSteps) { saveKinematicState(fixedTimeStep); @@ -198,6 +246,7 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo for (int i=0;i<clampedSimulationSteps;i++) { internalSingleStepSimulation(fixedTimeStep); + synchronizeMotionStates(); } } @@ -226,20 +275,23 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep) btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; + + ///solve non-contact constraints solveNoncontactConstraints(infoGlobal); ///solve contact constraints solveContactConstraints(infoGlobal); - ///update vehicle simulation - updateVehicles(timeStep); - ///CallbackTriggers(); ///integrate transforms integrateTransforms(timeStep); - + + ///update vehicle simulation + updateVehicles(timeStep); + + updateActivationState( timeStep ); @@ -268,12 +320,19 @@ void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) { - body->setGravity(m_gravity); - bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); - short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter; - short collisionFilterMask = isDynamic? btBroadphaseProxy::AllFilter : btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; + if (!body->isStaticOrKinematicObject()) + { + body->setGravity(m_gravity); + } - addCollisionObject(body,collisionFilterGroup,collisionFilterMask); + if (body->getCollisionShape()) + { + bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); + short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter; + short collisionFilterMask = isDynamic? btBroadphaseProxy::AllFilter : btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; + + addCollisionObject(body,collisionFilterGroup,collisionFilterMask); + } } @@ -303,12 +362,12 @@ void btDiscreteDynamicsWorld::updateActivationState(float timeStep) if (body->wantsSleeping()) { - if (body->GetActivationState() == ACTIVE_TAG) - body->SetActivationState( WANTS_DEACTIVATION ); + if (body->getActivationState() == ACTIVE_TAG) + body->setActivationState( WANTS_DEACTIVATION ); } else { - if (body->GetActivationState() != DISABLE_DEACTIVATION) - body->SetActivationState( ACTIVE_TAG ); + if (body->getActivationState() != DISABLE_DEACTIVATION) + body->setActivationState( ACTIVE_TAG ); } } } @@ -436,11 +495,11 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && ((colObj1) && ((colObj1)->mergesSimulationIslands()))) { - if (colObj0->IsActive() || colObj1->IsActive()) + if (colObj0->isActive() || colObj1->isActive()) { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->m_islandTag1, - (colObj1)->m_islandTag1); + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); } } } @@ -501,19 +560,19 @@ void btDiscreteDynamicsWorld::updateAabbs() // if (body->IsActive() && (!body->IsStatic())) { btPoint3 minAabb,maxAabb; - colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb); + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < 1e12f)) { - bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb); + bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb); } else { //something went wrong, investigate //this assert is unwanted in 3D modelers (danger of loosing work) assert(0); - body->SetActivationState(DISABLE_SIMULATION); + body->setActivationState(DISABLE_SIMULATION); static bool reportMe = true; if (reportMe) @@ -548,7 +607,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(float timeStep) btRigidBody* body = btRigidBody::upcast(colObj); if (body) { - if (body->IsActive() && (!body->isStaticOrKinematicObject())) + if (body->isActive() && (!body->isStaticOrKinematicObject())) { body->predictIntegratedTransform(timeStep, predictedTrans); body->proceedToTransform( predictedTrans); @@ -571,11 +630,11 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep) { if (!body->isStaticOrKinematicObject()) { - if (body->IsActive()) + if (body->isActive()) { body->applyForces( timeStep); body->integrateVelocities( timeStep); - body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); } } } @@ -768,3 +827,14 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, } } } + +void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +{ + if (m_ownsConstraintSolver) + { + delete m_constraintSolver; + } + m_ownsConstraintSolver = false; + m_constraintSolver = solver; +} + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 77e4ada645c..12939753442 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -86,7 +86,7 @@ public: btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver=0); ///this btDiscreteDynamicsWorld will create and own dispatcher, pairCache and constraintSolver, and deletes it in the destructor. - btDiscreteDynamicsWorld(); + btDiscreteDynamicsWorld(btConstraintSolver* constraintSolver = 0); virtual ~btDiscreteDynamicsWorld(); @@ -136,6 +136,7 @@ public: void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + virtual void setConstraintSolver(btConstraintSolver* solver); }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index fa916f65505..6023f29b4fb 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -19,6 +19,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionWorld.h" class btTypedConstraint; class btRaycastVehicle; +class btConstraintSolver; + ///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous class btDynamicsWorld : public btCollisionWorld @@ -65,6 +67,8 @@ class btDynamicsWorld : public btCollisionWorld virtual void removeRigidBody(btRigidBody* body) = 0; + virtual void setConstraintSolver(btConstraintSolver* solver) = 0; + }; #endif //BT_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index a434f5e41dd..db05f37d8c8 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -42,7 +42,13 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap m_frictionSolverType(0) { - motionState->getWorldTransform(m_worldTransform); + if (motionState) + { + motionState->getWorldTransform(m_worldTransform); + } else + { + m_worldTransform = btTransform::getIdentity(); + } m_interpolationWorldTransform = m_worldTransform; m_interpolationLinearVelocity.setValue(0,0,0); @@ -64,7 +70,7 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap } - +#ifdef OBSOLETE_MOTIONSTATE_LESS btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) : m_gravity(0.0f, 0.0f, 0.0f), @@ -100,30 +106,34 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi } +#endif //OBSOLETE_MOTIONSTATE_LESS + + #define EXPERIMENTAL_JITTER_REMOVAL 1 #ifdef EXPERIMENTAL_JITTER_REMOVAL -//Bullet 2.20b has experimental code to reduce jitter just before objects fall asleep/deactivate -//doesn't work very well yet (value 0 only reduces performance a bit, no difference in functionality) -float gClippedAngvelThresholdSqr = 0.f; -float gClippedLinearThresholdSqr = 0.f; +//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! +float gClippedAngvelThresholdSqr = 0.01f; +float gClippedLinearThresholdSqr = 0.01f; +float gJitterVelocityDampingFactor = 1.f; #endif //EXPERIMENTAL_JITTER_REMOVAL void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) { #ifdef EXPERIMENTAL_JITTER_REMOVAL - //clip to avoid jitter - if (m_angularVelocity.length2() < gClippedAngvelThresholdSqr) + //if (wantsSleeping()) { - m_angularVelocity.setValue(0,0,0); - printf("clipped!\n"); + //clip to avoid jitter + if ((m_angularVelocity.length2() < gClippedAngvelThresholdSqr) && + (m_linearVelocity.length2() < gClippedLinearThresholdSqr)) + { + m_angularVelocity *= gJitterVelocityDampingFactor; + m_linearVelocity *= gJitterVelocityDampingFactor; + } } - if (m_linearVelocity.length2() < gClippedLinearThresholdSqr) - { - m_linearVelocity.setValue(0,0,0); - printf("clipped!\n"); - } #endif //EXPERIMENTAL_JITTER_REMOVAL btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index fca3658cb31..c5fecdb87a6 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -62,7 +62,11 @@ class btRigidBody : public btCollisionObject public: +#ifdef OBSOLETE_MOTIONSTATE_LESS + //not supported, please use btMotionState btRigidBody(float mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f); +#endif //OBSOLETE_MOTIONSTATE_LESS + btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f); void proceedToTransform(const btTransform& newTrans); @@ -71,11 +75,11 @@ public: ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast static const btRigidBody* upcast(const btCollisionObject* colObj) { - return (const btRigidBody*)colObj->m_internalOwner; + return (const btRigidBody*)colObj->getInternalOwner(); } static btRigidBody* upcast(btCollisionObject* colObj) { - return (btRigidBody*)colObj->m_internalOwner; + return (btRigidBody*)colObj->getInternalOwner(); } /// continuous collision detection needs prediction @@ -247,7 +251,7 @@ public: inline void updateDeactivation(float timeStep) { - if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == DISABLE_DEACTIVATION)) + if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) return; if ((getLinearVelocity().length2() < gLinearSleepingThreshold*gLinearSleepingThreshold) && @@ -257,7 +261,7 @@ public: } else { m_deactivationTime=0.f; - SetActivationState(0); + setActivationState(0); } } @@ -265,14 +269,14 @@ public: inline bool wantsSleeping() { - if (GetActivationState() == DISABLE_DEACTIVATION) + if (getActivationState() == DISABLE_DEACTIVATION) return false; //disable deactivation if (gDisableDeactivation || (gDeactivationTime == 0.f)) return false; - if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == WANTS_DEACTIVATION)) + if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) return true; if (m_deactivationTime> gDeactivationTime) @@ -309,6 +313,8 @@ public: void setMotionState(btMotionState* motionState) { m_optionalMotionState = motionState; + if (m_optionalMotionState) + motionState->getWorldTransform(m_worldTransform); } //for experimental overriding of friction/contact solver func diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 53a088de750..dabf291669d 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -114,12 +114,12 @@ void btSimpleDynamicsWorld::updateAabbs() btRigidBody* body = btRigidBody::upcast(colObj); if (body) { - if (body->IsActive() && (!body->isStaticObject())) + if (body->isActive() && (!body->isStaticObject())) { btPoint3 minAabb,maxAabb; - colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb); + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); btBroadphaseInterface* bp = getBroadphase(); - bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb); + bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb); } } } @@ -134,7 +134,7 @@ void btSimpleDynamicsWorld::integrateTransforms(float timeStep) btRigidBody* body = btRigidBody::upcast(colObj); if (body) { - if (body->IsActive() && (!body->isStaticObject())) + if (body->isActive() && (!body->isStaticObject())) { body->predictIntegratedTransform(timeStep, predictedTrans); body->proceedToTransform( predictedTrans); @@ -155,11 +155,11 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(float timeStep) { if (!body->isStaticObject()) { - if (body->IsActive()) + if (body->isActive()) { body->applyForces( timeStep); body->integrateVelocities( timeStep); - body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); } } } @@ -176,11 +176,22 @@ void btSimpleDynamicsWorld::synchronizeMotionStates() btRigidBody* body = btRigidBody::upcast(colObj); if (body && body->getMotionState()) { - if (body->GetActivationState() != ISLAND_SLEEPING) + if (body->getActivationState() != ISLAND_SLEEPING) { - body->getMotionState()->setWorldTransform(body->m_worldTransform); + body->getMotionState()->setWorldTransform(body->getWorldTransform()); } } } } + + +void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +{ + if (m_ownsConstraintSolver) + { + delete m_constraintSolver; + } + m_ownsConstraintSolver = false; + m_constraintSolver = solver; +} diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index a65b56c4f57..f74e2460732 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -77,6 +77,8 @@ public: void synchronizeMotionStates(); + virtual void setConstraintSolver(btConstraintSolver* solver); + }; #endif //BT_SIMPLE_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index 7a2043e2b38..700bdb866cd 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -11,6 +11,7 @@ #include "LinearMath/btVector3.h" #include "btRaycastVehicle.h" + #include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" #include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" #include "LinearMath/btQuaternion.h" @@ -23,7 +24,7 @@ -static btRigidBody s_fixedObject( 0,btTransform(btQuaternion(0,0,0,1)),0); +static btRigidBody s_fixedObject( 0,0,0); btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) :m_vehicleRaycaster(raycaster), @@ -75,8 +76,8 @@ btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1]; - updateWheelTransformsWS( wheel ); - updateWheelTransform(getNumWheels()-1); + updateWheelTransformsWS( wheel , false ); + updateWheelTransform(getNumWheels()-1,false); return wheel; } @@ -91,15 +92,18 @@ const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const } -void btRaycastVehicle::updateWheelTransform( int wheelIndex ) +void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform) { btWheelInfo& wheel = m_wheelInfo[ wheelIndex ]; - updateWheelTransformsWS(wheel); + updateWheelTransformsWS(wheel,interpolatedTransform); btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS; btVector3 fwd = up.cross(right); fwd = fwd.normalize(); +// up = right.cross(fwd); +// up.normalize(); + //rotate around steering over de wheelAxleWS float steering = wheel.m_steering; @@ -138,16 +142,16 @@ void btRaycastVehicle::resetSuspension() } } -void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel ) +void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform) { wheel.m_raycastInfo.m_isInContact = false; - btTransform chassisTrans; - if (getRigidBody()->getMotionState()) + btTransform chassisTrans = getChassisWorldTransform(); + if (interpolatedTransform && (getRigidBody()->getMotionState())) + { getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); - else - chassisTrans = getRigidBody()->getCenterOfMassTransform(); - + } + wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; @@ -155,7 +159,7 @@ void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel ) btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) { - updateWheelTransformsWS( wheel ); + updateWheelTransformsWS( wheel,false); btScalar depth = -1; @@ -239,12 +243,35 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) } +const btTransform& btRaycastVehicle::getChassisWorldTransform() const +{ + /*if (getRigidBody()->getMotionState()) + { + btTransform chassisWorldTrans; + getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans); + return chassisWorldTrans; + } + */ + + + return getRigidBody()->getCenterOfMassTransform(); +} + + void btRaycastVehicle::updateVehicle( btScalar step ) { + { + for (int i=0;i<getNumWheels();i++) + { + updateWheelTransform(i,false); + } + } + m_currentVehicleSpeedKmHour = 3.6f * getRigidBody()->getLinearVelocity().length(); - const btTransform& chassisTrans = getRigidBody()->getCenterOfMassTransform(); + const btTransform& chassisTrans = getChassisWorldTransform(); + btVector3 forwardW ( chassisTrans.getBasis()[0][m_indexForwardAxis], chassisTrans.getBasis()[1][m_indexForwardAxis], @@ -304,10 +331,12 @@ void btRaycastVehicle::updateVehicle( btScalar step ) if (wheel.m_raycastInfo.m_isInContact) { + const btTransform& chassisWorldTransform = getChassisWorldTransform(); + btVector3 fwd ( - getRigidBody()->getCenterOfMassTransform().getBasis()[0][m_indexForwardAxis], - getRigidBody()->getCenterOfMassTransform().getBasis()[1][m_indexForwardAxis], - getRigidBody()->getCenterOfMassTransform().getBasis()[2][m_indexForwardAxis]); + chassisWorldTransform.getBasis()[0][m_indexForwardAxis], + chassisWorldTransform.getBasis()[1][m_indexForwardAxis], + chassisWorldTransform.getBasis()[2][m_indexForwardAxis]); btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h index a7534c67555..b928248cc2d 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -68,9 +68,8 @@ public: virtual ~btRaycastVehicle() ; - - - + const btTransform& getChassisWorldTransform() const; + btScalar rayCast(btWheelInfo& wheel); virtual void updateVehicle(btScalar step); @@ -86,7 +85,7 @@ public: const btTransform& getWheelTransformWS( int wheelIndex ) const; - void updateWheelTransform( int wheelIndex ); + void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true ); void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth); @@ -103,7 +102,7 @@ public: btWheelInfo& getWheelInfo(int index); - void updateWheelTransformsWS(btWheelInfo& wheel ); + void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true); void setBrake(float brake,int wheelIndex); diff --git a/extern/bullet2/src/LinearMath/CMakeLists.txt b/extern/bullet2/src/LinearMath/CMakeLists.txt index b9546167534..207eed94a3e 100644 --- a/extern/bullet2/src/LinearMath/CMakeLists.txt +++ b/extern/bullet2/src/LinearMath/CMakeLists.txt @@ -5,5 +5,6 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src } ADD_LIBRARY(LibLinearMath btQuickprof.cpp +btGeometryUtil.cpp ) diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet2/src/LinearMath/btAabbUtil2.h index 1cc7d4ebdf2..2eacb8e242b 100644 --- a/extern/bullet2/src/LinearMath/btAabbUtil2.h +++ b/extern/bullet2/src/LinearMath/btAabbUtil2.h @@ -18,6 +18,8 @@ subject to the following restrictions: #define AABB_UTIL2 #include "LinearMath/btVector3.h" +#include "LinearMath/btSimdMinMax.h" + #define btMin(a,b) ((a < b ? a : b)) #define btMax(a,b) ((a > b ? a : b)) @@ -53,5 +55,73 @@ SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices, return true; } + +SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent) +{ + return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) | + (p.getX() > halfExtent.getX() ? 0x08 : 0x0) | + (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) | + (p.getY() > halfExtent.getY() ? 0x10 : 0x0) | + (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) | + (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0); +} + + +SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& aabbMin, + const btVector3& aabbMax, + btScalar& param, btVector3& normal) +{ + btVector3 aabbHalfExtent = (aabbMax-aabbMin)* 0.5f; + btVector3 aabbCenter = (aabbMax+aabbMin)* 0.5f; + btVector3 source = rayFrom - aabbCenter; + btVector3 target = rayTo - aabbCenter; + int sourceOutcode = btOutcode(source,aabbHalfExtent); + int targetOutcode = btOutcode(target,aabbHalfExtent); + if ((sourceOutcode & targetOutcode) == 0x0) + { + btScalar lambda_enter = btScalar(0.0); + btScalar lambda_exit = param; + btVector3 r = target - source; + int i; + btScalar normSign = 1; + btVector3 hitNormal(0,0,0); + int bit=1; + + for (int j=0;j<2;j++) + { + for (i = 0; i != 3; ++i) + { + if (sourceOutcode & bit) + { + btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i]; + if (lambda_enter <= lambda) + { + lambda_enter = lambda; + hitNormal.setValue(0,0,0); + hitNormal[i] = normSign; + } + } + else if (targetOutcode & bit) + { + btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i]; + btSetMin(lambda_exit, lambda); + } + bit<<=1; + } + normSign = -1.f; + } + if (lambda_enter <= lambda_exit) + { + param = lambda_enter; + normal = hitNormal; + return true; + } + } + return false; +} + + #endif diff --git a/extern/bullet2/src/LinearMath/btDefaultMotionState.h b/extern/bullet2/src/LinearMath/btDefaultMotionState.h index 805631ac56f..6b85b37fb9e 100644 --- a/extern/bullet2/src/LinearMath/btDefaultMotionState.h +++ b/extern/bullet2/src/LinearMath/btDefaultMotionState.h @@ -19,16 +19,25 @@ struct btDefaultMotionState : public btMotionState } ///synchronizes world transform from user to physics - virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) + virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const { centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ; } ///synchronizes world transform from physics to user + ///Bullet only calls the update of worldtransform for active objects virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans) { m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ; } + + ///Bullet gives a callback for objects that are about to be deactivated (put asleep) + /// You can intercept this callback for your own bookkeeping. + ///Also you can return false to disable deactivation for this object this frame. + virtual bool deactivationCallback(void* userPointer) { + return true; + } + }; #endif //DEFAULT_MOTION_STATE_H diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp new file mode 100644 index 00000000000..69d061d27d0 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp @@ -0,0 +1,168 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / 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 "btGeometryUtil.h" + +bool btGeometryUtil::isPointInsidePlanes(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin) +{ + int numbrushes = planeEquations.size(); + for (int i=0;i<numbrushes;i++) + { + const btVector3& N1 = planeEquations[i]; + float dist = float(N1.dot(point))+float(N1[3])-margin; + if (dist>0.f) + { + return false; + } + } + return true; + +} + + +bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const std::vector<btVector3>& vertices, float margin) +{ + int numvertices = vertices.size(); + for (int i=0;i<numvertices;i++) + { + const btVector3& N1 = vertices[i]; + float dist = float(planeNormal.dot(N1))+float(planeNormal[3])-margin; + if (dist>0.f) + { + return false; + } + } + return true; +} + +bool notExist(const btVector3& planeEquation,const std::vector<btVector3>& planeEquations) +{ + int numbrushes = planeEquations.size(); + for (int i=0;i<numbrushes;i++) + { + const btVector3& N1 = planeEquations[i]; + if (planeEquation.dot(N1) > 0.999f) + { + return false; + } + } + return true; +} + +void btGeometryUtil::getPlaneEquationsFromVertices(std::vector<btVector3>& vertices, std::vector<btVector3>& planeEquationsOut ) +{ + const int numvertices = vertices.size(); + // brute force: + for (int i=0;i<numvertices;i++) + { + const btVector3& N1 = vertices[i]; + + + for (int j=i+1;j<numvertices;j++) + { + const btVector3& N2 = vertices[j]; + + for (int k=j+1;k<numvertices;k++) + { + + const btVector3& N3 = vertices[k]; + + btVector3 planeEquation,edge0,edge1; + edge0 = N2-N1; + edge1 = N3-N1; + float normalSign = 1.f; + for (int ww=0;ww<2;ww++) + { + planeEquation = normalSign * edge0.cross(edge1); + if (planeEquation.length2() > 0.0001f) + { + planeEquation.normalize(); + if (notExist(planeEquation,planeEquationsOut)) + { + planeEquation[3] = -planeEquation.dot(N1); + + //check if inside, and replace supportingVertexOut if needed + if (areVerticesBehindPlane(planeEquation,vertices,0.01f)) + { + planeEquationsOut.push_back(planeEquation); + } + } + } + normalSign = -1.f; + } + + } + } + } + +} + +void btGeometryUtil::getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut ) +{ + const int numbrushes = planeEquations.size(); + // brute force: + for (int i=0;i<numbrushes;i++) + { + const btVector3& N1 = planeEquations[i]; + + + for (int j=i+1;j<numbrushes;j++) + { + const btVector3& N2 = planeEquations[j]; + + for (int k=j+1;k<numbrushes;k++) + { + + const btVector3& N3 = planeEquations[k]; + + btVector3 n2n3; n2n3 = N2.cross(N3); + btVector3 n3n1; n3n1 = N3.cross(N1); + btVector3 n1n2; n1n2 = N1.cross(N2); + + if ( ( n2n3.length2() > 0.0001f ) && + ( n3n1.length2() > 0.0001f ) && + ( n1n2.length2() > 0.0001f ) ) + { + //point P out of 3 plane equations: + + // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) + //P = ------------------------------------------------------------------------- + // N1 . ( N2 * N3 ) + + + float quotient = (N1.dot(n2n3)); + if (btFabs(quotient) > 0.000001f) + { + quotient = -1.f / quotient; + n2n3 *= N1[3]; + n3n1 *= N2[3]; + n1n2 *= N3[3]; + btVector3 potentialVertex = n2n3; + potentialVertex += n3n1; + potentialVertex += n1n2; + potentialVertex *= quotient; + + //check if inside, and replace supportingVertexOut if needed + if (isPointInsidePlanes(planeEquations,potentialVertex,0.01f)) + { + verticesOut.push_back(potentialVertex); + } + } + } + } + } + } +}
\ No newline at end of file diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.h b/extern/bullet2/src/LinearMath/btGeometryUtil.h new file mode 100644 index 00000000000..3170c215008 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btGeometryUtil.h @@ -0,0 +1,39 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / 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_GEOMETRY_UTIL_H +#define BT_GEOMETRY_UTIL_H +#include <vector> +#include "btVector3.h" + +class btGeometryUtil +{ + public: + + + static void getPlaneEquationsFromVertices(std::vector<btVector3>& vertices, std::vector<btVector3>& planeEquationsOut ); + + static void getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut ); + + static bool isInside(const std::vector<btVector3>& vertices, const btVector3& planeNormal, float margin); + + static bool isPointInsidePlanes(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin); + + static bool areVerticesBehindPlane(const btVector3& planeNormal, const std::vector<btVector3>& vertices, float margin); + +}; + + +#endif //BT_GEOMETRY_UTIL_H
\ No newline at end of file diff --git a/extern/bullet2/src/LinearMath/btMotionState.h b/extern/bullet2/src/LinearMath/btMotionState.h index 4bbb3d44888..a9e212d3c71 100644 --- a/extern/bullet2/src/LinearMath/btMotionState.h +++ b/extern/bullet2/src/LinearMath/btMotionState.h @@ -29,10 +29,15 @@ class btMotionState } - virtual void getWorldTransform(btTransform& worldTrans )=0; + virtual void getWorldTransform(btTransform& worldTrans ) const =0; + //Bullet only calls the update of worldtransform for active objects virtual void setWorldTransform(const btTransform& worldTrans)=0; - + + //future: when Bullet makes attempt to deactivate object, you can intercept this callback (return false to disable deactivation for this object this frame) + virtual bool deactivationCallback(void* userPointer) { + return true; + } }; #endif //BT_MOTIONSTATE_H diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp index 23b8b8b2c40..4d013a65853 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.cpp +++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp @@ -1,25 +1,18 @@ -/************************************************************************ -* QuickProf * -* Copyright (C) 2006 * -* Tyler Streeter tylerstreeter@gmail.com * -* All rights reserved. * -* Web: http://quickprof.sourceforge.net * -* * -* This library is free software; you can redistribute it and/or * -* modify it under the terms of EITHER: * -* (1) The GNU Lesser bteral Public License as published by the Free * -* Software Foundation; either version 2.1 of the License, or (at * -* your option) any later version. The text of the GNU Lesser * -* bteral Public License is included with this library in the * -* file license-LGPL.txt. * -* (2) The BSD-style license that is included with this library in * -* the file license-BSD.txt. * -* * -* This library is distributed in the hope that it will be useful, * -* but WITHOUT ANY WARRANTY; without even the implied warranty of * -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * -* license-LGPL.txt and license-BSD.txt for more details. * -************************************************************************/ +/* +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. diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h index 801ef07a946..ec560c8e33e 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -1,26 +1,18 @@ -/************************************************************************ -* QuickProf * -* Copyright (C) 2006 * -* Tyler Streeter tylerstreeter@gmail.com * -* All rights reserved. * -* Web: http://quickprof.sourceforge.net * -* * -* This library is free software; you can redistribute it and/or * -* modify it under the terms of EITHER: * -* (1) The GNU Lesser bteral Public License as published by the Free * -* Software Foundation; either version 2.1 of the License, or (at * -* your option) any later version. The text of the GNU Lesser * -* bteral Public License is included with this library in the * -* file license-LGPL.txt. * -* (2) The BSD-style license that is included with this library in * -* the file license-BSD.txt. * -* * -* This library is distributed in the hope that it will be useful, * -* but WITHOUT ANY WARRANTY; without even the implied warranty of * -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * -* license-LGPL.txt and license-BSD.txt for more details. * -************************************************************************/ +/* +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. @@ -46,6 +38,10 @@ typedef uint64_t __int64; #endif +#if defined (SUNOS) || defined (__SUNOS__) + #include <stdio.h> +#endif + #if defined(WIN32) || defined(_WIN32) #define USE_WINDOWS_TIMERS #include <windows.h> diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index dea040a80bd..aa046b89df0 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -88,13 +88,13 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } #endif -const btScalar SIMD_2_PI = 6.283185307179586232f; -const btScalar SIMD_PI = SIMD_2_PI * btScalar(0.5f); -const btScalar SIMD_HALF_PI = SIMD_2_PI * btScalar(0.25f); -const btScalar SIMD_RADS_PER_DEG = SIMD_2_PI / btScalar(360.0f); -const btScalar SIMD_DEGS_PER_RAD = btScalar(360.0f) / SIMD_2_PI; -const btScalar SIMD_EPSILON = FLT_EPSILON; -const btScalar SIMD_INFINITY = FLT_MAX; +#define SIMD_2_PI 6.283185307179586232f +#define SIMD_PI (SIMD_2_PI * btScalar(0.5f)) +#define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25f)) +#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0f)) +#define SIMD_DEGS_PER_RAD (btScalar(360.0f) / SIMD_2_PI) +#define SIMD_EPSILON FLT_EPSILON +#define SIMD_INFINITY FLT_MAX SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } @@ -114,13 +114,14 @@ SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } */ -SIMD_FORCE_INLINE int btSign(btScalar x) { - return x < 0.0f ? -1 : x > 0.0f ? 1 : 0; +SIMD_FORCE_INLINE int btIsNegative(btScalar x) { + return x < 0.0f ? 1 : 0; } SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; } SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; } +#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name #endif //SIMD___SCALAR_H diff --git a/extern/bullet2/src/LinearMath/btSimdMinMax.h b/extern/bullet2/src/LinearMath/btSimdMinMax.h index 16c31552f88..2731c8b09f3 100644 --- a/extern/bullet2/src/LinearMath/btSimdMinMax.h +++ b/extern/bullet2/src/LinearMath/btSimdMinMax.h @@ -16,6 +16,7 @@ subject to the following restrictions: #ifndef SIMD_MINMAX_H #define SIMD_MINMAX_H +#include "LinearMath/btScalar.h" template <class T> SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) { diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index 12ca634c232..39fa830f4df 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -108,7 +108,16 @@ public: static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel) { linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; -#ifdef USE_QUATERNION_DIFF + btVector3 axis; + btScalar angle; + calculateDiffAxisAngle(transform0,transform1,axis,angle); + angVel = axis * angle / timeStep; + } + + static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle) + { + + #ifdef USE_QUATERNION_DIFF btQuaternion orn0 = transform0.getRotation(); btQuaternion orn1a = transform1.getRotation(); btQuaternion orn1 = orn0.farthest(orn1a); @@ -118,9 +127,7 @@ public: btQuaternion dorn; dmat.getRotation(dorn); #endif//USE_QUATERNION_DIFF - - btVector3 axis; - btScalar angle; + angle = dorn.getAngle(); axis = btVector3(dorn.x(),dorn.y(),dorn.z()); axis[3] = 0.f; @@ -130,13 +137,8 @@ public: axis = btVector3(1.f,0.f,0.f); else axis /= btSqrt(len); - - - angVel = axis * angle / timeStep; - } - }; #endif //SIMD_TRANSFORM_UTIL_H |