diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionDispatch')
29 files changed, 424 insertions, 323 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 0d76b1013f9..81133670f0c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -13,6 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +#include "LinearMath/btScalar.h" #include "SphereTriangleDetector.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -28,13 +29,14 @@ m_triangle(triangle) void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) { + (void)debugDraw; const btTransform& transformA = input.m_transformA; const btTransform& transformB = input.m_transformB; btVector3 point,normal; - btScalar timeOfImpact = 1.f; - btScalar depth = 0.f; -// output.m_distance = 1e30f; + btScalar timeOfImpact = btScalar(1.); + btScalar depth = btScalar(0.); +// output.m_distance = btScalar(1e30); //move sphere into triangle space btTransform sphereInTr = transformB.inverseTimes(transformA); @@ -45,19 +47,19 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res } -#define MAX_OVERLAP 0.f +#define MAX_OVERLAP btScalar(0.) // See also geometrictools.com // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv -float SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { +btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { btVector3 diff = p - from; btVector3 v = to - from; - float t = v.dot(diff); + btScalar t = v.dot(diff); if (t > 0) { - float dotVV = v.dot(v); + btScalar dotVV = v.dot(v); if (t < dotVV) { t /= dotVV; diff -= t*v; @@ -80,7 +82,7 @@ bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* ve } ///combined discrete/continuous sphere-triangle -bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, float &timeOfImpact) +bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact) { const btVector3* vertices = &m_triangle->getVertexPtr(0); @@ -92,25 +94,25 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); normal.normalize(); btVector3 p1ToCentre = c - vertices[0]; - float distanceFromPlane = p1ToCentre.dot(normal); + btScalar distanceFromPlane = p1ToCentre.dot(normal); - if (distanceFromPlane < 0.f) + if (distanceFromPlane < btScalar(0.)) { //triangle facing the other way - distanceFromPlane *= -1.f; - normal *= -1.f; + distanceFromPlane *= btScalar(-1.); + normal *= btScalar(-1.); } ///todo: move this gContactBreakingThreshold into a proper structure - extern float gContactBreakingThreshold; + extern btScalar gContactBreakingThreshold; - float contactMargin = gContactBreakingThreshold; + btScalar contactMargin = gContactBreakingThreshold; bool isInsideContactPlane = distanceFromPlane < r + contactMargin; bool isInsideShellPlane = distanceFromPlane < r; - float deltaDotNormal = delta.dot(normal); - if (!isInsideShellPlane && deltaDotNormal >= 0.0f) + btScalar deltaDotNormal = delta.dot(normal); + if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0)) return false; // Check for contact / intersection @@ -123,7 +125,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po contactPoint = c - normal*distanceFromPlane; } else { // Could be inside one of the contact capsules - float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); + btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); btVector3 nearestOnEdge; for (int i = 0; i < m_triangle->getNumEdges(); i++) { @@ -132,7 +134,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po m_triangle->getEdge(i,pa,pb); - float distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); + btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); if (distanceSqr < contactCapsuleRadiusSqr) { // Yep, we're inside a capsule hasContact = true; @@ -145,25 +147,22 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po if (hasContact) { btVector3 contactToCentre = c - contactPoint; - float distanceSqr = contactToCentre.length2(); + btScalar distanceSqr = contactToCentre.length2(); if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) { - float distance = btSqrt(distanceSqr); - if (1) - { - resultNormal = contactToCentre; - resultNormal.normalize(); - } + btScalar distance = btSqrt(distanceSqr); + resultNormal = contactToCentre; + resultNormal.normalize(); point = contactPoint; depth = -(r-distance); return true; } - if (delta.dot(contactToCentre) >= 0.0f) + if (delta.dot(contactToCentre) >= btScalar(0.0)) return false; // Moving towards the contact point -> collision point = contactPoint; - timeOfImpact = 0.0f; + timeOfImpact = btScalar(0.0); return true; } @@ -189,7 +188,7 @@ bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const b btVector3 edge2_normal( edge2.cross(normal)); btVector3 edge3_normal( edge3.cross(normal)); - float r1, r2, r3; + btScalar r1, r2, r3; r1 = edge1_normal.dot( p1_to_p ); r2 = edge2_normal.dot( p2_to_p ); r3 = edge3_normal.dot( p3_to_p ); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 8fedba19922..b32806a6846 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef SPHERE_TRIANGLE_DETECTOR_H #define SPHERE_TRIANGLE_DETECTOR_H -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btPoint3.h" +#include "../NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" +#include "../../LinearMath/btPoint3.h" class btSphereShape; @@ -36,7 +36,7 @@ struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface private: - bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, float &timeOfImpact); + bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact); bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ); bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index 1eaa4a0fd50..d51a59af7f0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef COLLISION_CREATE_FUNC #define COLLISION_CREATE_FUNC -#include <vector> - -typedef std::vector<class btCollisionObject*> btCollisionObjectArray; +#include "../../LinearMath/btAlignedObjectArray.h" +typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray; class btCollisionAlgorithm; class btCollisionObject; @@ -35,8 +34,11 @@ struct btCollisionAlgorithmCreateFunc } virtual ~btCollisionAlgorithmCreateFunc(){}; - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , btCollisionObject* body0,btCollisionObject* body1) { + + (void)body0; + (void)body1; return 0; } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 309c1890b40..198276d76df 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -25,7 +25,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include <algorithm> #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" int gNumManifold = 0; @@ -33,16 +32,17 @@ int gNumManifold = 0; #include <stdio.h> -btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms) -:m_useIslands(true), -m_convexConvexCreateFunc(0), +btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms): m_count(0), +m_useIslands(true), +m_convexConvexCreateFunc(0), m_convexConcaveCreateFunc(0), m_swappedConvexConcaveCreateFunc(0), m_compoundCreateFunc(0), m_swappedCompoundCreateFunc(0), m_emptyCreateFunc(0) { + (void)noDefaultAlgorithms; int i; setNearCallback(defaultNearCallback); @@ -56,11 +56,14 @@ m_emptyCreateFunc(0) } } } - +//if you want to not link with the default collision algorithms, you can +//define BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION +//in your Bullet library build system +#ifndef BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION btCollisionDispatcher::btCollisionDispatcher (): - m_useIslands(true), - m_count(0) + m_count(0), + m_useIslands(true) { int i; @@ -86,6 +89,9 @@ btCollisionDispatcher::btCollisionDispatcher (): }; +#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION + + void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) { m_doubleDispatch[proxyType0][proxyType1] = createFunc; @@ -129,20 +135,17 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) gNumManifold--; //printf("releaseManifold: gNumManifold %d\n",gNumManifold); - clearManifold(manifold); - std::vector<btPersistentManifold*>::iterator i = - std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold); - if (!(i == m_manifoldsPtr.end())) + ///todo: this can be improved a lot, linear search might be slow part! + int findIndex = m_manifoldsPtr.findLinearSearch(manifold); + if (findIndex < m_manifoldsPtr.size()) { - std::swap(*i, m_manifoldsPtr.back()); + m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); m_manifoldsPtr.pop_back(); delete manifold; - } - } @@ -164,6 +167,8 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo } +#ifndef BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION + btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(int proxyType0,int proxyType1) { @@ -197,6 +202,8 @@ btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(in return m_emptyCreateFunc; } +#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION + #ifndef USE_DISPATCH_REGISTRY_ARRAY @@ -288,6 +295,16 @@ public: { } + btCollisionPairCallback& operator=(btCollisionPairCallback& other) + { + m_dispatchInfo = other.m_dispatchInfo; + m_dispatcher = other.m_dispatcher; + return *this; + } + + virtual ~btCollisionPairCallback() {} + + virtual bool processOverlap(btBroadphasePair& pair) { (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); @@ -337,7 +354,7 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, } else { //continuous collision detection query, time of impact (toi) - float toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); + btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); if (dispatchInfo.m_timeOfImpact > toi) dispatchInfo.m_timeOfImpact = toi; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 77c09b618ec..ca5aba8f01c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -16,13 +16,13 @@ subject to the following restrictions: #ifndef COLLISION__DISPATCHER_H #define COLLISION__DISPATCHER_H -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "../BroadphaseCollision/btDispatcher.h" +#include "../NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" - -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "../CollisionDispatch/btManifoldResult.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../../LinearMath/btAlignedObjectArray.h" class btIDebugDraw; class btOverlappingPairCache; @@ -41,8 +41,9 @@ typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispa ///Time of Impact, Closest Points and Penetration Depth. class btCollisionDispatcher : public btDispatcher { + int m_count; - std::vector<btPersistentManifold*> m_manifoldsPtr; + btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; bool m_useIslands; @@ -68,7 +69,6 @@ class btCollisionDispatcher : public btDispatcher public: - ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); @@ -92,8 +92,6 @@ public: return m_manifoldsPtr[index]; } - int m_count; - ///the default constructor creates/register default collision algorithms, for convex, compound and concave shape support btCollisionDispatcher (); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 13fa0b5baa9..7b6f066275d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -20,11 +20,11 @@ btCollisionObject::btCollisionObject() m_collisionShape(0), m_collisionFlags(0), m_activationState1(1), - m_deactivationTime(0.f), + m_deactivationTime(btScalar(0.)), m_userObjectPointer(0), - m_hitFraction(1.f), - m_ccdSweptSphereRadius(0.f), - m_ccdSquareMotionThreshold(0.f) + m_hitFraction(btScalar(1.)), + m_ccdSweptSphereRadius(btScalar(0.)), + m_ccdSquareMotionThreshold(btScalar(0.)) { } @@ -46,7 +46,7 @@ void btCollisionObject::activate(bool forceActivation) if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT))) { setActivationState(ACTIVE_TAG); - m_deactivationTime = 0.f; + m_deactivationTime = btScalar(0.); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 9a0129ac29c..93990235afe 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef COLLISION_OBJECT_H #define COLLISION_OBJECT_H -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btTransform.h" //island management, m_activationState1 #define ACTIVE_TAG 1 @@ -27,21 +27,19 @@ subject to the following restrictions: struct btBroadphaseProxy; class btCollisionShape; -#include "LinearMath/btMotionState.h" +#include "../../LinearMath/btMotionState.h" /// 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. -class btCollisionObject +ATTRIBUTE_ALIGNED16(class) btCollisionObject { protected: btTransform m_worldTransform; - btBroadphaseProxy* m_broadphaseHandle; - btCollisionShape* m_collisionShape; ///m_interpolationWorldTransform is used for CCD and interpolation ///it can be either previous or future (predicted) transform @@ -50,12 +48,16 @@ protected: //without destroying the continuous interpolated motion (which uses this interpolation velocities) btVector3 m_interpolationLinearVelocity; btVector3 m_interpolationAngularVelocity; + btBroadphaseProxy* m_broadphaseHandle; + btCollisionShape* m_collisionShape; int m_collisionFlags; int m_islandTag1; + int m_companionId; + int m_activationState1; - float m_deactivationTime; + btScalar m_deactivationTime; btScalar m_friction; btScalar m_restitution; @@ -67,13 +69,15 @@ protected: void* m_internalOwner; ///time of impact calculation - float m_hitFraction; + btScalar m_hitFraction; ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - float m_ccdSweptSphereRadius; + btScalar m_ccdSweptSphereRadius; /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold - float m_ccdSquareMotionThreshold; + btScalar m_ccdSquareMotionThreshold; + + char m_pad[8]; public: @@ -82,7 +86,7 @@ public: 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) + CF_CUSTOM_MATERIAL_CALLBACK = 8//this allows per-triangle material (friction/restitution) }; @@ -137,11 +141,11 @@ public: void setActivationState(int newState); - void setDeactivationTime(float time) + void setDeactivationTime(btScalar time) { m_deactivationTime = time; } - float getDeactivationTime() const + btScalar getDeactivationTime() const { return m_deactivationTime; } @@ -155,19 +159,19 @@ public: return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } - void setRestitution(float rest) + void setRestitution(btScalar rest) { m_restitution = rest; } - float getRestitution() const + btScalar getRestitution() const { return m_restitution; } - void setFriction(float frict) + void setFriction(btScalar frict) { m_friction = frict; } - float getFriction() const + btScalar getFriction() const { return m_friction; } @@ -251,12 +255,22 @@ public: m_islandTag1 = tag; } - const float getHitFraction() const + const int getCompanionId() const + { + return m_companionId; + } + + void setCompanionId(int id) + { + m_companionId = id; + } + + const btScalar getHitFraction() const { return m_hitFraction; } - void setHitFraction(float hitFraction) + void setHitFraction(btScalar hitFraction) { m_hitFraction = hitFraction; } @@ -273,25 +287,25 @@ public: } ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - float getCcdSweptSphereRadius() const + btScalar getCcdSweptSphereRadius() const { return m_ccdSweptSphereRadius; } ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - void setCcdSweptSphereRadius(float radius) + void setCcdSweptSphereRadius(btScalar radius) { m_ccdSweptSphereRadius = radius; } - float getCcdSquareMotionThreshold() const + btScalar 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) + void setCcdSquareMotionThreshold(btScalar ccdSquareMotionThreshold) { m_ccdSquareMotionThreshold = ccdSquareMotionThreshold; } @@ -308,6 +322,7 @@ public: m_userObjectPointer = userPointer; } -}; +} +; #endif //COLLISION_OBJECT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index fa1561973fb..49f35c2cc90 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -31,7 +31,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include <algorithm> btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache, int stackSize) :m_dispatcher1(dispatcher), @@ -50,13 +49,10 @@ btCollisionWorld::~btCollisionWorld() delete m_stackAlloc; //clean up remaining objects - std::vector<btCollisionObject*>::iterator i; - - for (i=m_collisionObjects.begin(); - !(i==m_collisionObjects.end()); i++) - + int i; + for (i=0;i<m_collisionObjects.size();i++) { - btCollisionObject* collisionObject= (*i); + btCollisionObject* collisionObject= m_collisionObjects[i]; btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) @@ -89,9 +85,7 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho { //check that the object isn't already added - std::vector<btCollisionObject*>::iterator i = std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject); - assert(i == m_collisionObjects.end()); - + btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); m_collisionObjects.push_back(collisionObject); @@ -125,13 +119,13 @@ void btCollisionWorld::performDiscreteCollisionDetection() { btDispatcherInfo& dispatchInfo = getDispatchInfo(); - BEGIN_PROFILE("performDiscreteCollisionDetection"); + BEGIN_PROFILE("perform Broadphase Collision Detection"); //update aabb (of all moved objects) btVector3 aabbMin,aabbMax; - for (size_t i=0;i<m_collisionObjects.size();i++) + for (int i=0;i<m_collisionObjects.size();i++) { m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax); m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax); @@ -139,6 +133,11 @@ void btCollisionWorld::performDiscreteCollisionDetection() m_broadphasePairCache->refreshOverlappingPairs(); + + END_PROFILE("perform Broadphase Collision Detection"); + + BEGIN_PROFILE("performDiscreteCollisionDetection"); + btDispatcher* dispatcher = getDispatcher(); if (dispatcher) dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache,dispatchInfo); @@ -169,15 +168,13 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) } - std::vector<btCollisionObject*>::iterator i = std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject); - - if (!(i == m_collisionObjects.end())) - { - std::swap(*i, m_collisionObjects.back()); - m_collisionObjects.pop_back(); - } + //swapremove + m_collisionObjects.remove(collisionObject); + } + + void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, @@ -185,12 +182,13 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra RayResultCallback& resultCallback) { - btSphereShape pointShape(0.0f); + btSphereShape pointShape(btScalar(0.0)); + pointShape.setMargin(0.f); if (collisionShape->isConvex()) { btConvexCast::CastResult castResult; - castResult.m_fraction = 1.f;//?? + castResult.m_fraction = btScalar(1.);//?? btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; @@ -201,7 +199,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { //add hit - if (castResult.m_normal.length2() > 0.0001f) + if (castResult.m_normal.length2() > btScalar(0.0001)) { castResult.m_normal.normalize(); if (castResult.m_fraction < resultCallback.m_closestHitFraction) @@ -252,7 +250,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra } - virtual float reportHit(const btVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; @@ -321,19 +319,16 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD) - std::vector<btCollisionObject*>::iterator iter; - - for (iter=m_collisionObjects.begin(); - !(iter==m_collisionObjects.end()); iter++) + int i; + for (i=0;i<m_collisionObjects.size();i++) { - - btCollisionObject* collisionObject= (*iter); + btCollisionObject* collisionObject= m_collisionObjects[i]; //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); btVector3 collisionObjectAabbMin,collisionObjectAabbMax; collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - float hitLambda = 1.f; //could use resultCallback.m_closestHitFraction, but needs testing + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index bd09d8c4d5d..0b0286c50f4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -67,15 +67,12 @@ subject to the following restrictions: class btStackAlloc; class btCollisionShape; class btBroadphaseInterface; -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" #include "btCollisionObject.h" #include "btCollisionDispatcher.h" //for definition of btCollisionObjectArray -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" - -#include <vector> - - +#include "../BroadphaseCollision/btOverlappingPairCache.h" +#include "../../LinearMath/btAlignedObjectArray.h" ///CollisionWorld is interface and container for the collision detection class btCollisionWorld @@ -84,7 +81,7 @@ class btCollisionWorld protected: - std::vector<btCollisionObject*> m_collisionObjects; + btAlignedObjectArray<btCollisionObject*> m_collisionObjects; btDispatcher* m_dispatcher1; @@ -137,18 +134,18 @@ public: LocalRayResult(btCollisionObject* collisionObject, LocalShapeInfo* localShapeInfo, const btVector3& hitNormalLocal, - float hitFraction) + btScalar hitFraction) :m_collisionObject(collisionObject), - m_localShapeInfo(m_localShapeInfo), + m_localShapeInfo(localShapeInfo), m_hitNormalLocal(hitNormalLocal), m_hitFraction(hitFraction) { } - btCollisionObject* m_collisionObject; + btCollisionObject* m_collisionObject; LocalShapeInfo* m_localShapeInfo; - const btVector3& m_hitNormalLocal; - float m_hitFraction; + btVector3 m_hitNormalLocal; + btScalar m_hitFraction; }; @@ -158,17 +155,17 @@ public: virtual ~RayResultCallback() { } - float m_closestHitFraction; + btScalar m_closestHitFraction; bool HasHit() { - return (m_closestHitFraction < 1.f); + return (m_closestHitFraction < btScalar(1.)); } RayResultCallback() - :m_closestHitFraction(1.f) + :m_closestHitFraction(btScalar(1.)) { } - virtual float AddSingleResult(LocalRayResult& rayResult) = 0; + virtual btScalar AddSingleResult(LocalRayResult& rayResult) = 0; }; struct ClosestRayResultCallback : public RayResultCallback @@ -187,7 +184,7 @@ public: btVector3 m_hitPointWorld; btCollisionObject* m_collisionObject; - virtual float AddSingleResult(LocalRayResult& rayResult) + virtual btScalar AddSingleResult(LocalRayResult& rayResult) { //caller already does the filter on the m_closestHitFraction @@ -216,7 +213,7 @@ public: /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. /// This allows more customization. - void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 43887d26a45..2f96c2e13c5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -89,7 +89,7 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt } } -float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { btCollisionObject* colObj = m_isSwapped? body1 : body0; @@ -106,7 +106,7 @@ float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* bod //then use each overlapping node AABB against Tree0 //and vise versa. - float hitFraction = 1.f; + btScalar hitFraction = btScalar(1.); int numChildren = m_childCollisionAlgorithms.size(); int i; @@ -124,7 +124,7 @@ float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* bod colObj->setWorldTransform( newChildWorldTrans ); colObj->setCollisionShape( childShape ); - float frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); + btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); if (frac<hitFraction) { hitFraction = frac; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index fe2d8628656..7091b233b46 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -16,21 +16,21 @@ subject to the following restrictions: #ifndef COMPOUND_COLLISION_ALGORITHM_H #define COMPOUND_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btDispatcher.h" +#include "../BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "../NarrowPhaseCollision/btPersistentManifold.h" class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include <vector> +#include "../BroadphaseCollision/btBroadphaseProxy.h" #include "btCollisionCreateFunc.h" +#include "../../LinearMath/btAlignedObjectArray.h" /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes /// Place holder, not fully implemented yet class btCompoundCollisionAlgorithm : public btCollisionAlgorithm { - std::vector<btCollisionAlgorithm*> m_childCollisionAlgorithms; + btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms; bool m_isSwapped; public: @@ -41,7 +41,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); struct CreateFunc :public btCollisionAlgorithmCreateFunc { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 06adb3a04c8..24ceacfd40d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -95,7 +95,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); //btVector3 center = triangle[0] + triangle[1]+triangle[2]; - //center *= 0.333333f; + //center *= btScalar(0.333333); //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color); //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color); //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color); @@ -134,7 +134,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i -void btConvexTriangleCallback::setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { m_dispatchInfoPtr = &dispatchInfo; m_collisionMarginTriangle = collisionMarginTriangle; @@ -146,7 +146,7 @@ void btConvexTriangleCallback::setTimeStepAndCounters(float collisionMarginTrian 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; + btScalar extraMargin = collisionMarginTriangle; btVector3 extra(extraMargin,extraMargin,extraMargin); m_aabbMax += extra; @@ -176,7 +176,7 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* bod if (convexBody->getCollisionShape()->isConvex()) { - float collisionMarginTriangle = concaveShape->getMargin(); + btScalar collisionMarginTriangle = concaveShape->getMargin(); resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut); @@ -196,9 +196,10 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* bod } -float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { - + (void)resultOut; + (void)dispatchInfo; btCollisionObject* convexbody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; @@ -207,10 +208,10 @@ 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->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); + btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) { - return 1.f; + return btScalar(1.); } //const btVector3& from = convexbody->m_worldTransform.getOrigin(); @@ -227,11 +228,11 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject btTransform m_ccdSphereToTrans; btTransform m_meshTransform; - float m_ccdSphereRadius; - float m_hitFraction; + btScalar m_ccdSphereRadius; + btScalar m_hitFraction; - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,float ccdSphereRadius,float hitFraction) + LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) :m_ccdSphereFromTrans(from), m_ccdSphereToTrans(to), m_ccdSphereRadius(ccdSphereRadius), @@ -242,6 +243,8 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { + (void)partId; + (void)triangleIndex; //do a swept sphere for now btTransform ident; ident.setIdentity(); @@ -276,11 +279,11 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject rayAabbMin.setMin(convexToLocal.getOrigin()); btVector3 rayAabbMax = convexFromLocal.getOrigin(); rayAabbMax.setMax(convexToLocal.getOrigin()); - float ccdRadius0 = convexbody->getCcdSweptSphereRadius(); + btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - float curHitFraction = 1.f; //is this available? + btScalar curHitFraction = btScalar(1.); //is this available? LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, convexbody->getCcdSweptSphereRadius(),curHitFraction); @@ -304,6 +307,6 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject } } - return 1.f; + return btScalar(1.); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index afcb38c94ef..4915b6c20c8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -16,13 +16,13 @@ subject to the following restrictions: #ifndef CONVEX_CONCAVE_COLLISION_ALGORITHM_H #define CONVEX_CONCAVE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btDispatcher.h" +#include "../BroadphaseCollision/btBroadphaseInterface.h" +#include "../CollisionShapes/btTriangleCallback.h" +#include "../NarrowPhaseCollision/btPersistentManifold.h" class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" #include "btCollisionCreateFunc.h" ///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called. @@ -38,7 +38,7 @@ class btConvexTriangleCallback : public btTriangleCallback btDispatcher* m_dispatcher; const btDispatcherInfo* m_dispatchInfoPtr; - float m_collisionMarginTriangle; + btScalar m_collisionMarginTriangle; public: int m_triangleCount; @@ -47,7 +47,7 @@ int m_triangleCount; btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); - void setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); virtual ~btConvexTriangleCallback(); @@ -86,7 +86,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); void clearCache(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 09457aea3e9..9105fe20b49 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -43,23 +43,9 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#ifdef WIN32 -#if _MSC_VER >= 1310 -//only use SIMD Hull code under Win32 -#ifdef TEST_HULL -#define USE_HULL 1 -#endif //TEST_HULL -#endif //_MSC_VER -#endif //WIN32 -#ifdef USE_HULL -#include "NarrowPhaseCollision/Hull.h" -#include "NarrowPhaseCollision/HullContactCollector.h" - - -#endif //USE_HULL btConvexConvexAlgorithm::CreateFunc::CreateFunc() @@ -76,6 +62,14 @@ btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simp m_pdSolver = pdSolver; } +btConvexConvexAlgorithm::CreateFunc::~CreateFunc() +{ + if (m_ownsSolvers){ + delete m_simplexSolver; + delete m_pdSolver; + } +} + btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) : btCollisionAlgorithm(ci), m_gjkPairDetector(0,0,simplexSolver,pdSolver), @@ -83,6 +77,9 @@ m_ownManifold (false), m_manifoldPtr(mf), m_lowLevelOfDetail(false) { + (void)body0; + (void)body1; + } @@ -147,7 +144,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; input.m_stackAlloc = dispatchInfo.m_stackAllocator; -// input.m_maximumDistanceSquared = 1e30f; +// input.m_maximumDistanceSquared = btScalar(1e30); input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); @@ -160,24 +157,26 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl bool disableCcd = false; -float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)resultOut; + (void)dispatchInfo; ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0->m_worldTransform, - float resultFraction = 1.f; + btScalar resultFraction = btScalar(1.); - float squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); - float squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); + btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); + btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); if (squareMot0 < col0->getCcdSquareMotionThreshold() && squareMot1 < col1->getCcdSquareMotionThreshold()) return resultFraction; if (disableCcd) - return 1.f; + return btScalar(1.); //An adhoc way of testing the Continuous Collision Detection algorithms diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index ccfe6d5b8cb..cbea9a92b75 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -16,11 +16,11 @@ subject to the following restrictions: #ifndef CONVEX_CONVEX_ALGORITHM_H #define CONVEX_CONVEX_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../NarrowPhaseCollision/btGjkPairDetector.h" +#include "../NarrowPhaseCollision/btPersistentManifold.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "btCollisionCreateFunc.h" class btConvexPenetrationDepthSolver; @@ -44,7 +44,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); void setLowLevelOfDetail(bool useLowLevel); @@ -62,6 +62,7 @@ public: CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); CreateFunc(); + virtual ~CreateFunc(); virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp index 9bc106564af..936054387c4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp @@ -22,14 +22,13 @@ btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& c { } -void btEmptyAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btEmptyAlgorithm::processCollision (btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* ) { - } -float btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* ) { - return 1.f; + return btScalar(1.); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index e0e136250ac..b1a193d2cfd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -15,7 +15,7 @@ subject to the following restrictions: #ifndef EMPTY_ALGORITH #define EMPTY_ALGORITH -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" #include "btCollisionCreateFunc.h" #define ATTRIBUTE_ALIGNED(a) @@ -31,12 +31,14 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); struct CreateFunc :public btCollisionAlgorithmCreateFunc { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { + (void)body0; + (void)body1; return new btEmptyAlgorithm(ci); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index cd22f3dd91e..490acc0b611 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -27,7 +27,7 @@ inline btScalar calculateCombinedFriction(const btCollisionObject* body0,const b { btScalar friction = body0->getFriction() * body1->getFriction(); - const btScalar MAX_FRICTION = 10.f; + const btScalar MAX_FRICTION = btScalar(10.); if (friction < -MAX_FRICTION) friction = -MAX_FRICTION; if (friction > MAX_FRICTION) @@ -53,7 +53,7 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b } -void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) +void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { assert(m_manifoldPtr); //order in manifold needs to match @@ -63,15 +63,22 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b bool isSwapped = m_manifoldPtr->getBody0() != m_body0; - btTransform transAInv = isSwapped? m_rootTransB.inverse() : m_rootTransA.inverse(); - btTransform transBInv = isSwapped? m_rootTransA.inverse() : m_rootTransB.inverse(); - btVector3 pointA = pointInWorld + normalOnBInWorld * depth; - btVector3 localA = transAInv(pointA ); - btVector3 localB = transBInv(pointInWorld); - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + btVector3 localA; + btVector3 localB; + if (isSwapped) + { + localA = m_rootTransB.invXform(pointA ); + localB = m_rootTransA.invXform(pointInWorld); + } else + { + localA = m_rootTransA.invXform(pointA ); + localB = m_rootTransB.invXform(pointInWorld); + } + + btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); int insertIndex = m_manifoldPtr->getCacheEntry(newPt); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 3d70689e3eb..77192625513 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -23,7 +23,7 @@ class btManifoldPoint; #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btTransform.h" typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1); extern ContactAddedCallback gContactAddedCallback; @@ -68,7 +68,7 @@ public: m_index1=index1; } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth); + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 06a5f95143e..ac2e8554c3a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -8,8 +8,7 @@ #include "BulletCollision/CollisionDispatch/btCollisionWorld.h" #include <stdio.h> -#include <algorithm> - +#include "LinearMath/btQuickprof.h" btSimulationIslandManager::btSimulationIslandManager() { @@ -33,7 +32,7 @@ void btSimulationIslandManager::findUnions(btDispatcher* dispatcher) for (int i=0;i<dispatcher->getNumManifolds();i++) { const btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); - //static objects (invmass 0.f) don't merge ! + //static objects (invmass btScalar(0.)) don't merge ! const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); @@ -57,16 +56,15 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld // put the index into m_controllers into m_tag { - std::vector<btCollisionObject*>::iterator i; int index = 0; - for (i=colWorld->getCollisionObjectArray().begin(); - !(i==colWorld->getCollisionObjectArray().end()); i++) + int i; + for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) { - - btCollisionObject* collisionObject= (*i); + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; collisionObject->setIslandTag(index); - collisionObject->setHitFraction(1.f); + collisionObject->setCompanionId(-1); + collisionObject->setHitFraction(btScalar(1.)); index++; } @@ -88,20 +86,19 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col { - std::vector<btCollisionObject*>::iterator i; - int index = 0; - for (i=colWorld->getCollisionObjectArray().begin(); - !(i==colWorld->getCollisionObjectArray().end()); i++) + int i; + for (i=0;i<colWorld->getCollisionObjectArray().size();i++) { - btCollisionObject* collisionObject= (*i); - + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; if (collisionObject->mergesSimulationIslands()) { collisionObject->setIslandTag( m_unionFind.find(index) ); + collisionObject->setCompanionId(-1); } else { collisionObject->setIslandTag(-1); + collisionObject->setCompanionId(-2); } index++; } @@ -118,13 +115,21 @@ inline int getIslandId(const btPersistentManifold* lhs) } -bool btPersistentManifoldSortPredicate(const btPersistentManifold* lhs, const btPersistentManifold* rhs) + + +/// function object that routes calls to operator< +class btPersistentManifoldSortPredicate { - int rIslandId0,lIslandId0; - rIslandId0 = getIslandId(rhs); - lIslandId0 = getIslandId(lhs); - return lIslandId0 < rIslandId0; -} + public: + + SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) + { + return getIslandId(lhs) < getIslandId(rhs); + } +}; + + + // @@ -132,6 +137,22 @@ bool btPersistentManifoldSortPredicate(const btPersistentManifold* lhs, const bt // void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback) { + + + + /*if (0) + { + int maxNumManifolds = dispatcher->getNumManifolds(); + btCollisionDispatcher* colDis = (btCollisionDispatcher*)dispatcher; + btPersistentManifold** manifold = colDis->getInternalManifoldPointer(); + callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, 0); + return; + } + */ + + + BEGIN_PROFILE("islandUnionFindAndHeapSort"); + //we are going to sort the unionfind array, and store the element id in the size //afterwards, we clean unionfind, to make sure no-one uses it anymore @@ -139,9 +160,11 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, int numElem = getUnionFind().getNumElements(); int endIslandIndex=1; + int startIslandIndex; + //update the sleeping state for bodies, if all are sleeping - for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) { int islandId = getUnionFind().getElement(startIslandIndex).m_id; for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) @@ -224,7 +247,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, } } - std::vector<btPersistentManifold*> islandmanifold; + btAlignedObjectArray<btPersistentManifold*> islandmanifold; int i; int maxNumManifolds = dispatcher->getNumManifolds(); islandmanifold.reserve(maxNumManifolds); @@ -261,63 +284,74 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, // Sort manifolds, based on islands // Sort the vector using predicate and std::sort - std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + + //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) + islandmanifold.heapSort(btPersistentManifoldSortPredicate()); //now process all active islands (sets of manifolds for now) int startManifoldIndex = 0; int endManifoldIndex = 1; - int islandId; + //int islandId; + + END_PROFILE("islandUnionFindAndHeapSort"); + + btAlignedObjectArray<btCollisionObject*> islandBodies; - //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) - { - int islandId = getUnionFind().getElement(startIslandIndex).m_id; - - - bool islandSleeping = false; + //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated + for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + { + int islandId = getUnionFind().getElement(startIslandIndex).m_id; + + + bool islandSleeping = false; for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) { int i = getUnionFind().getElement(endIslandIndex).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; + islandBodies.push_back(colObj0); if (!colObj0->isActive()) islandSleeping = true; } - //find the accompanying contact manifold for this islandId - int numIslandManifolds = 0; - btPersistentManifold** startManifold = 0; + //find the accompanying contact manifold for this islandId + int numIslandManifolds = 0; + btPersistentManifold** startManifold = 0; - if (startManifoldIndex<numManifolds) - { - int curIslandId = getIslandId(islandmanifold[startManifoldIndex]); - if (curIslandId == islandId) - { - startManifold = &islandmanifold[startManifoldIndex]; - - for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++) - { + if (startManifoldIndex<numManifolds) + { + int curIslandId = getIslandId(islandmanifold[startManifoldIndex]); + if (curIslandId == islandId) + { + startManifold = &islandmanifold[startManifoldIndex]; + + for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++) + { - } - /// Process the actual simulation, only if not sleeping/deactivated - numIslandManifolds = endManifoldIndex-startManifoldIndex; - } + } + /// Process the actual simulation, only if not sleeping/deactivated + numIslandManifolds = endManifoldIndex-startManifoldIndex; + } - } + } - if (!islandSleeping) - { - callback->ProcessIsland(startManifold,numIslandManifolds, islandId); - } + if (!islandSleeping) + { + callback->ProcessIsland(&islandBodies[0],islandBodies.size(),startManifold,numIslandManifolds, islandId); + } + + if (numIslandManifolds) + { + startManifoldIndex = endManifoldIndex; + } - if (numIslandManifolds) - { - startManifoldIndex = endManifoldIndex; - } - } + islandBodies.resize(0); + } + } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index 68d9b8038d6..d91ed1c20eb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -16,9 +16,10 @@ subject to the following restrictions: #ifndef SIMULATION_ISLAND_MANAGER_H #define SIMULATION_ISLAND_MANAGER_H -#include "BulletCollision/CollisionDispatch/btUnionFind.h" +#include "../CollisionDispatch/btUnionFind.h" #include "btCollisionCreateFunc.h" +class btCollisionObject; class btCollisionWorld; class btDispatcher; @@ -49,7 +50,7 @@ public: { virtual ~IslandCallback() {}; - virtual void ProcessIsland(class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; + virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; }; void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index 1423c335407..05556bd34e2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -50,6 +50,8 @@ btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)dispatchInfo; + (void)resultOut; if (!m_manifoldPtr) return; @@ -64,7 +66,7 @@ void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,b btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin(); btScalar radius = sphere0->getRadius(); - float dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius); + btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius); if (dist < SIMD_EPSILON) { @@ -81,10 +83,15 @@ void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,b } -float btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + //not yet - return 1.f; + return btScalar(1.); } @@ -117,14 +124,14 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box ///////////////////////////////////////////////// btVector3 tmp, prel, n[6], normal, v3P; - btScalar fSep = 10000000.0f, fSepThis; + btScalar fSep = btScalar(10000000.0), fSepThis; - n[0].setValue( -1.0f, 0.0f, 0.0f ); - n[1].setValue( 0.0f, -1.0f, 0.0f ); - n[2].setValue( 0.0f, 0.0f, -1.0f ); - n[3].setValue( 1.0f, 0.0f, 0.0f ); - n[4].setValue( 0.0f, 1.0f, 0.0f ); - n[5].setValue( 0.0f, 0.0f, 1.0f ); + n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) ); + n[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) ); + n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) ); + n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) ); + n[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) ); + n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) ); // convert point in local space prel = m44T.invXform( sphereCenter); @@ -136,7 +143,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box for (int i=0;i<6;i++) { int j = i<3? 0:1; - if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > 0.0f ) + if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) ) { v3P = v3P - n[i]*fSepThis; bFound = true; @@ -154,9 +161,9 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box pointOnBox = v3P + normal*margins; v3PointOnSphere = prel - normal*fRadius; - if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > 0.0f ) + if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) ) { - return 1.0f; + return btScalar(1.0); } // transform back in world space @@ -171,7 +178,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box { fSep = - btSqrt(fSeps2); normal = (pointOnBox-v3PointOnSphere); - normal *= 1.f/fSep; + normal *= btScalar(1.)/fSep; } return fSep; @@ -185,10 +192,10 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box bounds[0] = boundsVec[0]; bounds[1] = boundsVec[1]; - if ( fPenetration <= 0.0f ) + if ( fPenetration <= btScalar(0.0) ) return (fPenetration-margins); else - return 1.0f; + return btScalar(1.0); } btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) @@ -200,14 +207,14 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* bounds[1] = aabbMax; btVector3 p0, tmp, prel, n[6], normal; - btScalar fSep = -10000000.0f, fSepThis; + btScalar fSep = btScalar(-10000000.0), fSepThis; - n[0].setValue( -1.0f, 0.0f, 0.0f ); - n[1].setValue( 0.0f, -1.0f, 0.0f ); - n[2].setValue( 0.0f, 0.0f, -1.0f ); - n[3].setValue( 1.0f, 0.0f, 0.0f ); - n[4].setValue( 0.0f, 1.0f, 0.0f ); - n[5].setValue( 0.0f, 0.0f, 1.0f ); + n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) ); + n[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) ); + n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) ); + n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) ); + n[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) ); + n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) ); const btTransform& m44T = boxObj->getWorldTransform(); @@ -219,7 +226,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* for (int i=0;i<6;i++) { int j = i<3 ? 0:1; - if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > 0.0f ) return 1.0f; + if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) ) return btScalar(1.0); if ( fSepThis > fSep ) { p0 = bounds[j]; normal = (btVector3&)n[i]; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index 68915a43e6f..07592909200 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -16,11 +16,11 @@ subject to the following restrictions: #ifndef SPHERE_BOX_COLLISION_ALGORITHM_H #define SPHERE_BOX_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" /// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. /// Other features are frame-coherency (persistent data) and collision response. @@ -38,7 +38,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius ); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index eb05bf974c5..424ff432f84 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -41,6 +41,8 @@ btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm() void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)dispatchInfo; + if (!m_manifoldPtr) return; @@ -48,7 +50,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape(); btVector3 diff = col0->getWorldTransform().getOrigin()- col1->getWorldTransform().getOrigin(); - float len = diff.length(); + btScalar len = diff.length(); btScalar radius0 = sphere0->getRadius(); btScalar radius1 = sphere1->getRadius(); @@ -71,8 +73,13 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 } -float btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)col0; + (void)col1; + (void)dispatchInfo; + (void)resultOut; + //not yet - return 1.f; + return btScalar(1.); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index 8b08d015aec..7a19ff31edf 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -16,9 +16,9 @@ subject to the following restrictions: #ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H #define SPHERE_SPHERE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. @@ -37,7 +37,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); virtual ~btSphereSphereCollisionAlgorithm(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 6f74c488659..b011b707e3f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -56,7 +56,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co SphereTriangleDetector detector(sphere,triangle); btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = 1e30f;//todo: tighter bounds + input.m_maximumDistanceSquared = btScalar(1e30);//todo: tighter bounds input.m_transformA = col0->getWorldTransform(); input.m_transformB = col1->getWorldTransform(); @@ -64,8 +64,13 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co } -float btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + //not yet - return 1.f; + return btScalar(1.); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index 82e4c5b37f1..57c6e6af619 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -16,9 +16,9 @@ subject to the following restrictions: #ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H #define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "../BroadphaseCollision/btCollisionAlgorithm.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. @@ -38,7 +38,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); virtual ~btSphereTriangleCollisionAlgorithm(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index 046f348a147..62254335796 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -15,7 +15,6 @@ subject to the following restrictions: #include "btUnionFind.h" #include <assert.h> -#include <algorithm> @@ -50,11 +49,16 @@ void btUnionFind::reset(int N) } } -bool btUnionFindElementSortPredicate(const btElement& lhs, const btElement& rhs) + +class btUnionFindElementSortPredicate { - return lhs.m_id < rhs.m_id; -} + public: + bool operator() ( const btElement& lhs, const btElement& rhs ) + { + return lhs.m_id < rhs.m_id; + } +}; ///this is a special operation, destroying the content of btUnionFind. ///it sorts the elements, based on island id, in order to make it easy to iterate over islands @@ -71,7 +75,9 @@ void btUnionFind::sortIslands() } // Sort the vector using predicate and std::sort - std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); + //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); + //perhaps use radix sort? + m_elements.heapSort(btUnionFindElementSortPredicate()); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index b1baca9ff15..236cc33b94f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -16,7 +16,10 @@ subject to the following restrictions: #ifndef UNION_FIND_H #define UNION_FIND_H -#include <vector> +#include "../../LinearMath/btAlignedObjectArray.h" + + #define USE_PATH_COMPRESSION 1 + struct btElement { int m_id; @@ -29,7 +32,7 @@ struct btElement class btUnionFind { private: - std::vector<btElement> m_elements; + btAlignedObjectArray<btElement> m_elements; public: @@ -78,6 +81,7 @@ class btUnionFind if (i == j) return; +#ifndef USE_PATH_COMPRESSION //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) if (m_elements[i].m_sz < m_elements[j].m_sz) { @@ -87,6 +91,9 @@ class btUnionFind { m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; } +#else + m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; +#endif //USE_PATH_COMPRESSION } int find(int x) @@ -97,7 +104,7 @@ class btUnionFind while (x != m_elements[x].m_id) { //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically - #define USE_PATH_COMPRESSION 1 + #ifdef USE_PATH_COMPRESSION // m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id; |