From ca26aeb7b23e37e65f49d907ea53fcaeee77ad4e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 23 Jun 2007 05:28:07 +0000 Subject: upgrade to latest Bullet 2.53. cross the fingers it doesn't break one of the exotic or less exotic platforms --- .../BroadphaseCollision/btAxisSweep3.cpp | 261 +++++-- .../BroadphaseCollision/btAxisSweep3.h | 83 +- .../BroadphaseCollision/btBroadphaseInterface.h | 2 +- .../BroadphaseCollision/btBroadphaseProxy.h | 40 +- .../BroadphaseCollision/btCollisionAlgorithm.h | 5 +- .../BroadphaseCollision/btDispatcher.h | 10 +- .../BroadphaseCollision/btOverlappingPairCache.cpp | 53 +- .../BroadphaseCollision/btOverlappingPairCache.h | 72 +- .../BroadphaseCollision/btSimpleBroadphase.cpp | 13 +- .../BroadphaseCollision/btSimpleBroadphase.h | 1 + .../CollisionDispatch/SphereTriangleDetector.cpp | 55 +- .../CollisionDispatch/SphereTriangleDetector.h | 6 +- .../CollisionDispatch/btCollisionCreateFunc.h | 10 +- .../CollisionDispatch/btCollisionDispatcher.cpp | 47 +- .../CollisionDispatch/btCollisionDispatcher.h | 16 +- .../CollisionDispatch/btCollisionObject.cpp | 10 +- .../CollisionDispatch/btCollisionObject.h | 61 +- .../CollisionDispatch/btCollisionWorld.cpp | 55 +- .../CollisionDispatch/btCollisionWorld.h | 35 +- .../btCompoundCollisionAlgorithm.cpp | 6 +- .../btCompoundCollisionAlgorithm.h | 16 +- .../btConvexConcaveCollisionAlgorithm.cpp | 31 +- .../btConvexConcaveCollisionAlgorithm.h | 18 +- .../CollisionDispatch/btConvexConvexAlgorithm.cpp | 39 +- .../CollisionDispatch/btConvexConvexAlgorithm.h | 13 +- .../btEmptyCollisionAlgorithm.cpp | 7 +- .../CollisionDispatch/btEmptyCollisionAlgorithm.h | 6 +- .../CollisionDispatch/btManifoldResult.cpp | 23 +- .../CollisionDispatch/btManifoldResult.h | 4 +- .../btSimulationIslandManager.cpp | 150 ++-- .../CollisionDispatch/btSimulationIslandManager.h | 5 +- .../btSphereBoxCollisionAlgorithm.cpp | 55 +- .../btSphereBoxCollisionAlgorithm.h | 10 +- .../btSphereSphereCollisionAlgorithm.cpp | 13 +- .../btSphereSphereCollisionAlgorithm.h | 8 +- .../btSphereTriangleCollisionAlgorithm.cpp | 11 +- .../btSphereTriangleCollisionAlgorithm.h | 8 +- .../CollisionDispatch/btUnionFind.cpp | 16 +- .../CollisionDispatch/btUnionFind.h | 13 +- .../BulletCollision/CollisionShapes/btBoxShape.cpp | 15 +- .../BulletCollision/CollisionShapes/btBoxShape.h | 50 +- .../CollisionShapes/btBvhTriangleMeshShape.cpp | 58 +- .../CollisionShapes/btBvhTriangleMeshShape.h | 35 +- .../CollisionShapes/btCapsuleShape.cpp | 146 ++++ .../CollisionShapes/btCapsuleShape.h | 60 ++ .../CollisionShapes/btCollisionMargin.h | 2 +- .../CollisionShapes/btCollisionShape.cpp | 36 +- .../CollisionShapes/btCollisionShape.h | 31 +- .../CollisionShapes/btCompoundShape.cpp | 26 +- .../CollisionShapes/btCompoundShape.h | 15 +- .../CollisionShapes/btConcaveShape.cpp | 2 +- .../CollisionShapes/btConcaveShape.h | 11 +- .../CollisionShapes/btConeShape.cpp | 16 +- .../BulletCollision/CollisionShapes/btConeShape.h | 24 +- .../CollisionShapes/btConvexHullShape.cpp | 38 +- .../CollisionShapes/btConvexHullShape.h | 26 +- .../CollisionShapes/btConvexShape.cpp | 22 +- .../CollisionShapes/btConvexShape.h | 46 +- .../CollisionShapes/btConvexTriangleMeshShape.cpp | 48 +- .../CollisionShapes/btConvexTriangleMeshShape.h | 6 +- .../CollisionShapes/btCylinderShape.cpp | 26 +- .../CollisionShapes/btCylinderShape.h | 32 +- .../CollisionShapes/btEmptyShape.cpp | 4 +- .../BulletCollision/CollisionShapes/btEmptyShape.h | 9 +- .../CollisionShapes/btHeightfieldTerrainShape.cpp | 25 +- .../CollisionShapes/btHeightfieldTerrainShape.h | 2 +- .../CollisionShapes/btMinkowskiSumShape.cpp | 5 +- .../CollisionShapes/btMinkowskiSumShape.h | 4 +- .../CollisionShapes/btMultiSphereShape.cpp | 22 +- .../CollisionShapes/btMultiSphereShape.h | 2 +- .../CollisionShapes/btOptimizedBvh.cpp | 714 +++++++++++++++-- .../CollisionShapes/btOptimizedBvh.h | 288 ++++++- .../CollisionShapes/btPolyhedralConvexShape.cpp | 52 +- .../CollisionShapes/btPolyhedralConvexShape.h | 44 +- .../CollisionShapes/btSphereShape.cpp | 13 +- .../CollisionShapes/btSphereShape.h | 8 +- .../CollisionShapes/btStaticPlaneShape.cpp | 19 +- .../CollisionShapes/btStaticPlaneShape.h | 2 +- .../CollisionShapes/btStridingMeshInterface.cpp | 53 +- .../CollisionShapes/btStridingMeshInterface.h | 6 +- .../CollisionShapes/btTetrahedronShape.cpp | 8 +- .../CollisionShapes/btTetrahedronShape.h | 4 +- .../CollisionShapes/btTriangleBuffer.h | 3 +- .../CollisionShapes/btTriangleCallback.h | 2 +- .../CollisionShapes/btTriangleIndexVertexArray.cpp | 6 +- .../CollisionShapes/btTriangleIndexVertexArray.h | 57 +- .../CollisionShapes/btTriangleMesh.cpp | 27 +- .../CollisionShapes/btTriangleMesh.h | 43 +- .../CollisionShapes/btTriangleMeshShape.cpp | 26 +- .../CollisionShapes/btTriangleMeshShape.h | 16 +- .../CollisionShapes/btTriangleShape.h | 9 +- .../btContinuousConvexCollision.cpp | 36 +- .../NarrowPhaseCollision/btConvexCast.h | 14 +- .../btConvexPenetrationDepthSolver.h | 2 +- .../btDiscreteCollisionDetectorInterface.h | 13 +- .../NarrowPhaseCollision/btGjkConvexCast.cpp | 16 +- .../NarrowPhaseCollision/btGjkConvexCast.h | 4 +- .../NarrowPhaseCollision/btGjkEpa.h | 2 +- .../btGjkEpaPenetrationDepthSolver.cpp | 5 +- .../NarrowPhaseCollision/btGjkPairDetector.cpp | 39 +- .../NarrowPhaseCollision/btGjkPairDetector.h | 5 +- .../NarrowPhaseCollision/btManifoldPoint.h | 27 +- .../btMinkowskiPenetrationDepthSolver.cpp | 111 +-- .../NarrowPhaseCollision/btPersistentManifold.cpp | 8 +- .../NarrowPhaseCollision/btPersistentManifold.h | 43 +- .../NarrowPhaseCollision/btPointCollector.h | 8 +- .../NarrowPhaseCollision/btRaycastCallback.cpp | 24 +- .../NarrowPhaseCollision/btRaycastCallback.h | 6 +- .../btSimplexSolverInterface.h | 4 +- .../btSubSimplexConvexCast.cpp | 30 +- .../btVoronoiSimplexSolver.cpp | 135 ++-- .../NarrowPhaseCollision/btVoronoiSimplexSolver.h | 12 +- .../ConstraintSolver/btConstraintSolver.h | 6 +- .../ConstraintSolver/btContactConstraint.cpp | 100 ++- .../ConstraintSolver/btContactConstraint.h | 57 +- .../ConstraintSolver/btContactSolverInfo.h | 30 +- .../ConstraintSolver/btGeneric6DofConstraint.cpp | 78 +- .../ConstraintSolver/btGeneric6DofConstraint.h | 12 +- .../ConstraintSolver/btHingeConstraint.cpp | 35 +- .../ConstraintSolver/btHingeConstraint.h | 11 +- .../ConstraintSolver/btJacobianEntry.h | 18 +- .../ConstraintSolver/btPoint2PointConstraint.cpp | 7 +- .../ConstraintSolver/btPoint2PointConstraint.h | 13 +- .../btSequentialImpulseConstraintSolver.cpp | 842 ++++++++++++++++++--- .../btSequentialImpulseConstraintSolver.h | 37 +- .../ConstraintSolver/btSolve2LinearConstraint.cpp | 58 +- .../ConstraintSolver/btSolve2LinearConstraint.h | 7 +- .../BulletDynamics/ConstraintSolver/btSolverBody.h | 71 ++ .../ConstraintSolver/btSolverConstraint.h | 63 ++ .../ConstraintSolver/btTypedConstraint.cpp | 12 +- .../ConstraintSolver/btTypedConstraint.h | 14 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 220 +++--- .../Dynamics/btDiscreteDynamicsWorld.h | 26 +- .../src/BulletDynamics/Dynamics/btDynamicsWorld.h | 16 +- .../src/BulletDynamics/Dynamics/btRigidBody.cpp | 94 +-- .../src/BulletDynamics/Dynamics/btRigidBody.h | 48 +- .../Dynamics/btSimpleDynamicsWorld.cpp | 30 +- .../Dynamics/btSimpleDynamicsWorld.h | 6 +- .../BulletDynamics/Vehicle/btRaycastVehicle.cpp | 173 +++-- .../src/BulletDynamics/Vehicle/btRaycastVehicle.h | 57 +- .../BulletDynamics/Vehicle/btVehicleRaycaster.h | 4 +- .../src/BulletDynamics/Vehicle/btWheelInfo.cpp | 13 +- .../src/BulletDynamics/Vehicle/btWheelInfo.h | 24 +- extern/bullet2/src/LinearMath/btAabbUtil2.h | 10 +- .../bullet2/src/LinearMath/btAlignedAllocator.cpp | 27 +- extern/bullet2/src/LinearMath/btAlignedAllocator.h | 7 +- .../bullet2/src/LinearMath/btAlignedObjectArray.h | 226 +++++- .../bullet2/src/LinearMath/btDefaultMotionState.h | 7 +- extern/bullet2/src/LinearMath/btGeometryUtil.cpp | 45 +- extern/bullet2/src/LinearMath/btGeometryUtil.h | 6 +- extern/bullet2/src/LinearMath/btIDebugDraw.h | 35 +- extern/bullet2/src/LinearMath/btMatrix3x3.h | 202 ++--- extern/bullet2/src/LinearMath/btMotionState.h | 7 +- extern/bullet2/src/LinearMath/btPoint3.h | 2 +- extern/bullet2/src/LinearMath/btQuadWord.h | 30 +- extern/bullet2/src/LinearMath/btQuaternion.h | 48 +- extern/bullet2/src/LinearMath/btQuickprof.cpp | 2 +- extern/bullet2/src/LinearMath/btQuickprof.h | 109 +-- extern/bullet2/src/LinearMath/btScalar.h | 56 +- extern/bullet2/src/LinearMath/btSimdMinMax.h | 2 +- extern/bullet2/src/LinearMath/btTransform.h | 26 +- extern/bullet2/src/LinearMath/btTransformUtil.h | 51 +- extern/bullet2/src/LinearMath/btVector3.h | 33 +- extern/bullet2/src/btBulletCollisionCommon.h | 1 + 164 files changed, 5013 insertions(+), 2176 deletions(-) create mode 100644 extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp create mode 100644 extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h create mode 100644 extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h create mode 100644 extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h (limited to 'extern') diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp index b05285ca727..be4a11506df 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp @@ -21,9 +21,34 @@ #include +#ifdef DEBUG_BROADPHASE +#include +void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinality) +{ + int numEdges = m_pHandles[0].m_maxEdges[axis]; + printf("SAP Axis %d, numEdges=%d\n",axis,numEdges); + + int i; + for (i=0;im_handle); + int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis]; + char beginOrEnd; + beginOrEnd=pEdge->IsMax()?'E':'B'; + printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex); + } + + if (checkCardinality) + assert(numEdges == m_numHandles*2+1); +} +#endif //DEBUG_BROADPHASE + + btBroadphaseProxy* btAxisSweep3::createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask) { - unsigned short handleId = addHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask); + (void)shapeType; + BP_FP_INT_TYPE handleId = addHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask); Handle* handle = getHandle(handleId); @@ -40,6 +65,7 @@ void btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,con { Handle* handle = static_cast(proxy); updateHandle(handle->m_handleId,aabbMin,aabbMax); + } @@ -50,10 +76,11 @@ void btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,con btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles) :btOverlappingPairCache() { + m_invalidPair = 0; //assert(bounds.HasVolume()); // 1 handle is reserved as sentinel - assert(maxHandles > 1 && maxHandles < 32767); + btAssert(maxHandles > 1 && maxHandles < BP_MAX_HANDLES); // init bounds m_worldAabbMin = worldAabbMin; @@ -61,7 +88,9 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; - m_quantize = btVector3(65535.0f,65535.0f,65535.0f) / aabbSize; + BP_FP_INT_TYPE maxInt = BP_HANDLE_SENTINEL; + + m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize; // allocate handles buffer and put all handles on free list m_pHandles = new Handle[maxHandles]; @@ -71,7 +100,7 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab // handle 0 is reserved as the null index, and is also used as the sentinel m_firstFreeHandle = 1; { - for (int i = m_firstFreeHandle; i < maxHandles; i++) + for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++) m_pHandles[i].SetNextFree(i + 1); m_pHandles[maxHandles - 1].SetNextFree(0); } @@ -94,9 +123,14 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab m_pEdges[axis][0].m_pos = 0; m_pEdges[axis][0].m_handle = 0; - m_pEdges[axis][1].m_pos = 0xffff; + m_pEdges[axis][1].m_pos = BP_HANDLE_SENTINEL; m_pEdges[axis][1].m_handle = 0; +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + } + } btAxisSweep3::~btAxisSweep3() @@ -107,43 +141,36 @@ btAxisSweep3::~btAxisSweep3() delete[] m_pHandles; } -void btAxisSweep3::quantize(unsigned short* out, const btPoint3& point, int isMax) const +void btAxisSweep3::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const { btPoint3 clampedPoint(point); - /* - if (isMax) - clampedPoint += btVector3(10,10,10); - else - { - clampedPoint -= btVector3(10,10,10); - } - */ + clampedPoint.setMax(m_worldAabbMin); clampedPoint.setMin(m_worldAabbMax); btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize; - out[0] = (unsigned short)(((int)v.getX() & 0xfffc) | isMax); - out[1] = (unsigned short)(((int)v.getY() & 0xfffc) | isMax); - out[2] = (unsigned short)(((int)v.getZ() & 0xfffc) | isMax); + out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & BP_HANDLE_MASK) | isMax); + out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & BP_HANDLE_MASK) | isMax); + out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & BP_HANDLE_MASK) | isMax); } -unsigned short btAxisSweep3::allocHandle() +BP_FP_INT_TYPE btAxisSweep3::allocHandle() { assert(m_firstFreeHandle); - unsigned short handle = m_firstFreeHandle; + BP_FP_INT_TYPE handle = m_firstFreeHandle; m_firstFreeHandle = getHandle(handle)->GetNextFree(); m_numHandles++; return handle; } -void btAxisSweep3::freeHandle(unsigned short handle) +void btAxisSweep3::freeHandle(BP_FP_INT_TYPE handle) { assert(handle > 0 && handle < m_maxHandles); @@ -155,15 +182,15 @@ void btAxisSweep3::freeHandle(unsigned short handle) -unsigned short btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask) +BP_FP_INT_TYPE btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask) { // quantize the bounds - unsigned short min[3], max[3]; + BP_FP_INT_TYPE min[3], max[3]; quantize(min, aabbMin, 0); quantize(max, aabbMax, 1); // allocate a handle - unsigned short handle = allocHandle(); + BP_FP_INT_TYPE handle = allocHandle(); assert(handle!= 0xcdcd); Handle* pHandle = getHandle(handle); @@ -175,11 +202,13 @@ unsigned short btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& a pHandle->m_collisionFilterMask = collisionFilterMask; // compute current limit of edge arrays - int limit = m_numHandles * 2; + BP_FP_INT_TYPE limit = m_numHandles * 2; + // insert new edges just inside the max boundary edge - for (int axis = 0; axis < 3; axis++) + for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++) { + m_pHandles[0].m_maxEdges[axis] += 2; m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1]; @@ -202,14 +231,14 @@ unsigned short btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& a sortMinDown(2, pHandle->m_minEdges[2], true); sortMaxDown(2, pHandle->m_maxEdges[2], true); - //PrintAxis(1); return handle; } -void btAxisSweep3::removeHandle(unsigned short handle) +void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle) { + Handle* pHandle = getHandle(handle); //explicitly remove the pairs containing the proxy @@ -220,42 +249,145 @@ void btAxisSweep3::removeHandle(unsigned short handle) // compute current limit of edge arrays int limit = m_numHandles * 2; + int axis; for (axis = 0;axis<3;axis++) { - Edge* pEdges = m_pEdges[axis]; - int maxEdge= pHandle->m_maxEdges[axis]; - pEdges[maxEdge].m_pos = 0xffff; - int minEdge = pHandle->m_minEdges[axis]; - pEdges[minEdge].m_pos = 0xffff; + m_pHandles[0].m_maxEdges[axis] -= 2; } // remove the edges by sorting them up to the end of the list for ( axis = 0; axis < 3; axis++) { Edge* pEdges = m_pEdges[axis]; - int max = pHandle->m_maxEdges[axis]; - pEdges[max].m_pos = 0xffff; + BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis]; + pEdges[max].m_pos = BP_HANDLE_SENTINEL; sortMaxUp(axis,max,false); - - int i = pHandle->m_minEdges[axis]; - pEdges[i].m_pos = 0xffff; + + + BP_FP_INT_TYPE i = pHandle->m_minEdges[axis]; + pEdges[i].m_pos = BP_HANDLE_SENTINEL; + sortMinUp(axis,i,false); pEdges[limit-1].m_handle = 0; - pEdges[limit-1].m_pos = 0xffff; + pEdges[limit-1].m_pos = BP_HANDLE_SENTINEL; + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis,false); +#endif //DEBUG_BROADPHASE + } + // free the handle freeHandle(handle); } +extern int gOverlappingPairs; + + +void btAxisSweep3::refreshOverlappingPairs() +{ + +} +void btAxisSweep3::processAllOverlappingPairs(btOverlapCallback* callback) +{ + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + m_overlappingPairArray.heapSort(btBroadphasePairSortPredicate()); + + //remove the 'invalid' ones +#ifdef USE_POPBACK_REMOVAL + while (m_invalidPair>0) + { + m_invalidPair--; + m_overlappingPairArray.pop_back(); + } +#else + m_overlappingPairArray.resize(m_overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; +#endif + + + int i; + + btBroadphasePair previousPair; + previousPair.m_pProxy0 = 0; + previousPair.m_pProxy1 = 0; + previousPair.m_algorithm = 0; + + + for (i=0;iprocessOverlap(pair); + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + cleanOverlappingPair(pair); + + // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + // m_overlappingPairArray.pop_back(); + pair.m_pProxy0 = 0; + pair.m_pProxy1 = 0; + m_invalidPair++; + gOverlappingPairs--; + } + + } +} + + +bool btAxisSweep3::testOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +{ + const Handle* pHandleA = static_cast(proxy0); + const Handle* pHandleB = static_cast(proxy1); + + //optimization 1: check the array index (memory address), instead of the m_pos + + for (int axis = 0; axis < 3; axis++) + { + if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || + pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) + { + return false; + } + } + return true; +} + bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB) { //optimization 1: check the array index (memory address), instead of the m_pos @@ -272,7 +404,7 @@ bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Hand } } - //optimization 2: only 2 axis need to be tested + //optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization) /*for (int axis = 0; axis < 3; axis++) { @@ -287,7 +419,7 @@ bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Hand return true; } -void btAxisSweep3::updateHandle(unsigned short handle, const btPoint3& aabbMin,const btPoint3& aabbMax) +void btAxisSweep3::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax) { // assert(bounds.IsFinite()); //assert(bounds.HasVolume()); @@ -295,15 +427,15 @@ void btAxisSweep3::updateHandle(unsigned short handle, const btPoint3& aabbMin,c Handle* pHandle = getHandle(handle); // quantize the new bounds - unsigned short min[3], max[3]; + BP_FP_INT_TYPE min[3], max[3]; quantize(min, aabbMin, 0); quantize(max, aabbMax, 1); // update changed edges for (int axis = 0; axis < 3; axis++) { - unsigned short emin = pHandle->m_minEdges[axis]; - unsigned short emax = pHandle->m_maxEdges[axis]; + BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis]; + BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis]; int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos; int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos; @@ -324,14 +456,22 @@ void btAxisSweep3::updateHandle(unsigned short handle, const btPoint3& aabbMin,c if (dmax < 0) sortMaxDown(axis, emax); + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE } - //PrintAxis(1); + } + + + // sorting a min edge downwards can only ever *add* overlaps -void btAxisSweep3::sortMinDown(int axis, unsigned short edge, bool updateOverlaps) +void btAxisSweep3::sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps) { + Edge* pEdge = m_pEdges[axis] + edge; Edge* pPrev = pEdge - 1; Handle* pHandleEdge = getHandle(pEdge->m_handle); @@ -368,16 +508,21 @@ void btAxisSweep3::sortMinDown(int axis, unsigned short edge, bool updateOverlap pEdge--; pPrev--; } + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + } // sorting a min edge upwards can only ever *remove* overlaps -void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps) +void btAxisSweep3::sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps) { Edge* pEdge = m_pEdges[axis] + edge; Edge* pNext = pEdge + 1; Handle* pHandleEdge = getHandle(pEdge->m_handle); - while (pEdge->m_pos > pNext->m_pos) + while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) { Handle* pHandleNext = getHandle(pNext->m_handle); @@ -386,10 +531,12 @@ void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps) // if next edge is maximum remove any overlap between the two handles if (updateOverlaps) { + /* Handle* handle0 = getHandle(pEdge->m_handle); Handle* handle1 = getHandle(pNext->m_handle); btBroadphasePair tmpPair(*handle0,*handle1); removeOverlappingPair(tmpPair); + */ } @@ -410,11 +557,14 @@ void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps) pEdge++; pNext++; } + + } // sorting a max edge downwards can only ever *remove* overlaps -void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlaps) +void btAxisSweep3::sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps) { + Edge* pEdge = m_pEdges[axis] + edge; Edge* pPrev = pEdge - 1; Handle* pHandleEdge = getHandle(pEdge->m_handle); @@ -428,6 +578,8 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap // if previous edge was a minimum remove any overlap between the two handles if (updateOverlaps) { + //this is done during the overlappingpairarray iteration/narrowphase collision + /* Handle* handle0 = getHandle(pEdge->m_handle); Handle* handle1 = getHandle(pPrev->m_handle); btBroadphasePair* pair = findPair(handle0,handle1); @@ -437,6 +589,8 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap { removeOverlappingPair(*pair); } + */ + } // update edge reference in other handle @@ -456,16 +610,22 @@ void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlap pEdge--; pPrev--; } + + +#ifdef DEBUG_BROADPHASE + debugPrintAxis(axis); +#endif //DEBUG_BROADPHASE + } // sorting a max edge upwards can only ever *add* overlaps -void btAxisSweep3::sortMaxUp(int axis, unsigned short edge, bool updateOverlaps) +void btAxisSweep3::sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps) { Edge* pEdge = m_pEdges[axis] + edge; Edge* pNext = pEdge + 1; Handle* pHandleEdge = getHandle(pEdge->m_handle); - while (pEdge->m_pos > pNext->m_pos) + while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) { Handle* pHandleNext = getHandle(pNext->m_handle); @@ -496,4 +656,5 @@ void btAxisSweep3::sortMaxUp(int axis, unsigned short edge, bool updateOverlaps) pEdge++; pNext++; } + } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index ebbbe01bbe6..57bbb368672 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -19,11 +19,29 @@ #ifndef AXIS_SWEEP_3_H #define AXIS_SWEEP_3_H -#include "LinearMath/btPoint3.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btPoint3.h" +#include "../../LinearMath/btVector3.h" #include "btOverlappingPairCache.h" #include "btBroadphaseProxy.h" + +//Enable BP_USE_FIXEDPOINT_INT_32 if you need more then 32767 objects +//#define BP_USE_FIXEDPOINT_INT_32 1 + +#ifdef BP_USE_FIXEDPOINT_INT_32 + #define BP_FP_INT_TYPE unsigned int + #define BP_MAX_HANDLES 1500000 //arbitrary maximum number of handles + #define BP_HANDLE_SENTINEL 0x7fffffff + #define BP_HANDLE_MASK 0xfffffffe +#else + #define BP_FP_INT_TYPE unsigned short int + #define BP_MAX_HANDLES 32767 + #define BP_HANDLE_SENTINEL 0xffff + #define BP_HANDLE_MASK 0xfffe +#endif //BP_USE_FIXEDPOINT_INT_32 + +//#define DEBUG_BROADPHASE 1 + /// btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase. /// It uses arrays rather then lists for storage of the 3 axis. Also it operates using integer coordinates instead of floats. /// The testOverlap check is optimized to check the array index, rather then the actual AABB coordinates/pos @@ -36,10 +54,10 @@ public: class Edge { public: - unsigned short m_pos; // low bit is min/max - unsigned short m_handle; + BP_FP_INT_TYPE m_pos; // low bit is min/max + BP_FP_INT_TYPE m_handle; - unsigned short IsMax() const {return m_pos & 1;} + BP_FP_INT_TYPE IsMax() const {return m_pos & 1;} }; public: @@ -48,14 +66,14 @@ public: public: // indexes into the edge arrays - unsigned short m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 - unsigned short m_handleId; - unsigned short m_pad; + BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 + BP_FP_INT_TYPE m_handleId; + BP_FP_INT_TYPE m_pad; //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject - inline void SetNextFree(unsigned short next) {m_minEdges[0] = next;} - inline unsigned short GetNextFree() const {return m_minEdges[0];} + inline void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;} + inline BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];} }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry @@ -65,51 +83,56 @@ private: btVector3 m_quantize; // scaling factor for quantization - int m_numHandles; // number of active handles + BP_FP_INT_TYPE m_numHandles; // number of active handles int m_maxHandles; // max number of handles Handle* m_pHandles; // handles pool - unsigned short m_firstFreeHandle; // free handles list + BP_FP_INT_TYPE m_firstFreeHandle; // free handles list Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries) + int m_invalidPair; // allocation/deallocation - unsigned short allocHandle(); - void freeHandle(unsigned short handle); + BP_FP_INT_TYPE allocHandle(); + void freeHandle(BP_FP_INT_TYPE handle); bool testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB); - //Overlap* AddOverlap(unsigned short handleA, unsigned short handleB); - //void RemoveOverlap(unsigned short handleA, unsigned short handleB); +#ifdef DEBUG_BROADPHASE + void debugPrintAxis(int axis,bool checkCardinality=true); +#endif //DEBUG_BROADPHASE - void quantize(unsigned short* out, const btPoint3& point, int isMax) const; + //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); + //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); - void sortMinDown(int axis, unsigned short edge, bool updateOverlaps = true); - void sortMinUp(int axis, unsigned short edge, bool updateOverlaps = true); - void sortMaxDown(int axis, unsigned short edge, bool updateOverlaps = true); - void sortMaxUp(int axis, unsigned short edge, bool updateOverlaps = true); + void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const; + + void sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true); + void sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true); + void sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true); + void sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true); public: btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384); virtual ~btAxisSweep3(); - virtual void refreshOverlappingPairs() - { - //this is replace by sweep and prune - } + virtual void refreshOverlappingPairs(); - unsigned short addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask); - void removeHandle(unsigned short handle); - void updateHandle(unsigned short handle, const btPoint3& aabbMin,const btPoint3& aabbMax); - inline Handle* getHandle(unsigned short index) const {return m_pHandles + index;} + BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask); + void removeHandle(BP_FP_INT_TYPE handle); + void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax); + inline Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;} + void processAllOverlappingPairs(btOverlapCallback* callback); //Broadphase Interface virtual btBroadphaseProxy* createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask); virtual void destroyProxy(btBroadphaseProxy* proxy); virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax); + bool testOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); }; -#endif //AXIS_SWEEP_3_H +#endif + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index 0c0bfe4f7b9..b6ace03c07a 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -21,7 +21,7 @@ subject to the following restrictions: struct btDispatcherInfo; class btDispatcher; struct btBroadphaseProxy; -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" ///BroadphaseInterface for aabb-overlapping object pairs class btBroadphaseInterface diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index b279022c802..40d9748ffa9 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -16,6 +16,7 @@ subject to the following restrictions: #ifndef BROADPHASE_PROXY_H #define BROADPHASE_PROXY_H +#include "../../LinearMath/btScalar.h" //for SIMD_FORCE_INLINE /// btDispatcher uses these types @@ -33,6 +34,7 @@ enum BroadphaseNativeTypes IMPLICIT_CONVEX_SHAPES_START_HERE, SPHERE_SHAPE_PROXYTYPE, MULTI_SPHERE_SHAPE_PROXYTYPE, + CAPSULE_SHAPE_PROXYTYPE, CONE_SHAPE_PROXYTYPE, CONVEX_SHAPE_PROXYTYPE, CYLINDER_SHAPE_PROXYTYPE, @@ -71,7 +73,7 @@ struct btBroadphaseProxy KinematicFilter = 4, DebrisFilter = 8, SensorTrigger = 16, - AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger, + AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger }; //Usually the client btCollisionObject or Rigidbody class @@ -113,7 +115,8 @@ struct btBroadphaseProxy return (proxyType == STATIC_PLANE_PROXYTYPE); } -}; +} +; class btCollisionAlgorithm; @@ -128,14 +131,16 @@ struct btBroadphasePair : m_pProxy0(0), m_pProxy1(0), - m_algorithm(0) + m_algorithm(0), + m_userInfo(0) { } btBroadphasePair(const btBroadphasePair& other) : m_pProxy0(other.m_pProxy0), m_pProxy1(other.m_pProxy1), - m_algorithm(other.m_algorithm) + m_algorithm(other.m_algorithm), + m_userInfo(other.m_userInfo) { } btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) @@ -154,6 +159,7 @@ struct btBroadphasePair } m_algorithm = 0; + m_userInfo = 0; } @@ -161,15 +167,37 @@ struct btBroadphasePair btBroadphaseProxy* m_pProxy1; mutable btCollisionAlgorithm* m_algorithm; -}; + mutable void* m_userInfo; +}; +/* //comparison for set operation, see Solid DT_Encounter -inline bool operator<(const btBroadphasePair& a, const btBroadphasePair& b) +SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePair& b) { return a.m_pProxy0 < b.m_pProxy0 || (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 < b.m_pProxy1); } +*/ + + +class btBroadphasePairSortPredicate +{ + public: + + bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) + { + return a.m_pProxy0 > b.m_pProxy0 || + (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) || + (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); + } +}; + + +SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) +{ + return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1); +} #endif //BROADPHASE_PROXY_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h index f9e22057058..55cec386a7b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -16,6 +16,8 @@ subject to the following restrictions: #ifndef COLLISION_ALGORITHM_H #define COLLISION_ALGORITHM_H +#include "../../LinearMath/btScalar.h" + struct btBroadphaseProxy; class btDispatcher; class btManifoldResult; @@ -34,6 +36,7 @@ struct btCollisionAlgorithmConstructionInfo btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp) :m_dispatcher(dispatcher) { + (void)temp; } btDispatcher* m_dispatcher; @@ -66,7 +69,7 @@ public: virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; - virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index c7714f592c4..3d958cc8fef 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -16,6 +16,8 @@ subject to the following restrictions: #ifndef _DISPATCHER_H #define _DISPATCHER_H +#include "../../LinearMath/btScalar.h" + class btCollisionAlgorithm; struct btBroadphaseProxy; class btRigidBody; @@ -34,10 +36,10 @@ struct btDispatcherInfo DISPATCH_CONTINUOUS }; btDispatcherInfo() - :m_timeStep(0.f), + :m_timeStep(btScalar(0.)), m_stepCount(0), m_dispatchFunc(DISPATCH_DISCRETE), - m_timeOfImpact(1.f), + m_timeOfImpact(btScalar(1.)), m_useContinuous(false), m_debugDraw(0), m_enableSatConvex(false), @@ -46,10 +48,10 @@ struct btDispatcherInfo { } - float m_timeStep; + btScalar m_timeStep; int m_stepCount; int m_dispatchFunc; - float m_timeOfImpact; + btScalar m_timeOfImpact; bool m_useContinuous; class btIDebugDraw* m_debugDraw; bool m_enableSatConvex; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 5e3fa633589..4187a8f2970 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -39,15 +39,15 @@ btOverlappingPairCache::~btOverlappingPairCache() void btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair) { - std::set::iterator it = m_overlappingPairSet.find(findPair); -// assert(it != m_overlappingPairSet.end()); - - if (it != m_overlappingPairSet.end()) + int findIndex = m_overlappingPairArray.findLinearSearch(findPair); + if (findIndex < m_overlappingPairArray.size()) { gOverlappingPairs--; - btBroadphasePair* pair = (btBroadphasePair*)(&(*it)); - cleanOverlappingPair(*pair); - m_overlappingPairSet.erase(it); + btBroadphasePair& pair = m_overlappingPairArray[findIndex]; + cleanOverlappingPair(pair); + + m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1); + m_overlappingPairArray.pop_back(); } } @@ -78,7 +78,7 @@ void btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa btBroadphasePair pair(*proxy0,*proxy1); - m_overlappingPairSet.insert(pair); + m_overlappingPairArray.push_back(pair); gOverlappingPairs++; } @@ -93,13 +93,15 @@ void btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa return 0; btBroadphasePair tmpPair(*proxy0,*proxy1); - std::set::iterator it = m_overlappingPairSet.find(tmpPair); - if ((it == m_overlappingPairSet.end())) - return 0; + int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); - //assert(it != m_overlappingPairSet.end()); - btBroadphasePair* pair = (btBroadphasePair*)(&(*it)); - return pair; + if (findIndex < m_overlappingPairArray.size()) + { + //assert(it != m_overlappingPairSet.end()); + btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; + return pair; + } + return 0; } @@ -170,30 +172,23 @@ void btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseP void btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback) { - std::set::iterator it = m_overlappingPairSet.begin(); - for (; !(it==m_overlappingPairSet.end());) + + int i; + + for (i=0;iprocessOverlap(*pair)) { cleanOverlappingPair(*pair); - std::set::iterator it2 = it; - //why does next line not compile under OS X?? -#ifdef MAC_OSX_FIXED_STL_SET - it2++; - it = m_overlappingPairSet.erase(it); - assert(it == it2); -#else - it++; - m_overlappingPairSet.erase(it2); -#endif //MAC_OSX_FIXED_STL_SET - + m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + m_overlappingPairArray.pop_back(); gOverlappingPairs--; } else { - it++; + i++; } } } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index bc62961bf3c..e3442212171 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -20,8 +20,8 @@ subject to the following restrictions: #include "btBroadphaseInterface.h" #include "btBroadphaseProxy.h" -#include "LinearMath/btPoint3.h" -#include +#include "../../LinearMath/btPoint3.h" +#include "../../LinearMath/btAlignedObjectArray.h" struct btOverlapCallback @@ -37,47 +37,61 @@ virtual ~btOverlapCallback() ///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase class btOverlappingPairCache : public btBroadphaseInterface { - //avoid brute-force finding all the time - std::set m_overlappingPairSet; - - //during the dispatch, check that user doesn't destroy/create proxy - bool m_blockedForChanges; - - public: + protected: + //avoid brute-force finding all the time + btAlignedObjectArray m_overlappingPairArray; + + //during the dispatch, check that user doesn't destroy/create proxy + bool m_blockedForChanges; - btOverlappingPairCache(); - virtual ~btOverlappingPairCache(); + public: + + btOverlappingPairCache(); + virtual ~btOverlappingPairCache(); - void processAllOverlappingPairs(btOverlapCallback*); + virtual void processAllOverlappingPairs(btOverlapCallback*); - void removeOverlappingPair(btBroadphasePair& pair); + void removeOverlappingPair(btBroadphasePair& pair); - void cleanOverlappingPair(btBroadphasePair& pair); - - void addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + void cleanOverlappingPair(btBroadphasePair& pair); + + void addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + - - void cleanProxyFromPairs(btBroadphaseProxy* proxy); + void cleanProxyFromPairs(btBroadphaseProxy* proxy); - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy); + void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy); - inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const - { - bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; - collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); - - return collides; - } + inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const + { + bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + - - virtual void refreshOverlappingPairs() =0; + virtual void refreshOverlappingPairs() =0; + btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } + }; #endif //OVERLAPPING_PAIR_CACHE_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index 6281e93eeb4..30bcbe0c5f1 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btMatrix3x3.h" -#include +#include void btSimpleBroadphase::validate() @@ -85,8 +85,8 @@ btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& min, cons btSimpleBroadphaseProxy* proxy1 = &m_proxies[0]; - int index = proxy - proxy1; - assert(index == freeIndex); + int index = int(proxy - proxy1); + btAssert(index == freeIndex); m_pProxies[m_numProxies] = proxy; m_numProxies++; @@ -100,7 +100,8 @@ class RemovingOverlapCallback : public btOverlapCallback protected: virtual bool processOverlap(btBroadphasePair& pair) { - assert(0); + (void)pair; + btAssert(0); return false; } }; @@ -131,8 +132,8 @@ void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg) btSimpleBroadphaseProxy* proxy0 = static_cast(proxyOrg); btSimpleBroadphaseProxy* proxy1 = &m_proxies[0]; - int index = proxy0 - proxy1; - assert (index < m_maxProxies); + int index = int(proxy0 - proxy1); + btAssert (index < m_maxProxies); m_freeProxies[--m_firstFreeProxy] = index; removeOverlappingPairsContainingProxy(proxyOrg); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index 281677081d7..1f265f3dd6b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -31,6 +31,7 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask), m_min(minpt),m_max(maxpt) { + (void)shapeType; } 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 - -typedef std::vector btCollisionObjectArray; +#include "../../LinearMath/btAlignedObjectArray.h" +typedef btAlignedObjectArray 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 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" int gNumManifold = 0; @@ -33,16 +32,17 @@ int gNumManifold = 0; #include -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::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 m_manifoldsPtr; + btAlignedObjectArray 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 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::iterator i; - - for (i=m_collisionObjects.begin(); - !(i==m_collisionObjects.end()); i++) - + int i; + for (i=0;igetBroadphaseHandle(); if (bp) @@ -89,9 +85,7 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho { //check that the object isn't already added - std::vector::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;igetCollisionShape()->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::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::iterator iter; - - for (iter=m_collisionObjects.begin(); - !(iter==m_collisionObjects.end()); iter++) + int i; + for (i=0;iGetRigidcollisionObject(); 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 - - +#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 m_collisionObjects; + btAlignedObjectArray 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 +#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 m_childCollisionAlgorithms; + btAlignedObjectArray 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(m_convexBody->getCollisionShape()); //CollisionShape* triangleShape = static_cast(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 -#include - +#include "LinearMath/btQuickprof.h" btSimulationIslandManager::btSimulationIslandManager() { @@ -33,7 +32,7 @@ void btSimulationIslandManager::findUnions(btDispatcher* dispatcher) for (int i=0;igetNumManifolds();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(manifold->getBody0()); const btCollisionObject* colObj1 = static_cast(manifold->getBody1()); @@ -57,16 +56,15 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld // put the index into m_controllers into m_tag { - std::vector::iterator i; int index = 0; - for (i=colWorld->getCollisionObjectArray().begin(); - !(i==colWorld->getCollisionObjectArray().end()); i++) + int i; + for (i=0;igetCollisionObjectArray().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::iterator i; - int index = 0; - for (i=colWorld->getCollisionObjectArray().begin(); - !(i==colWorld->getCollisionObjectArray().end()); i++) + int i; + for (i=0;igetCollisionObjectArray().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 islandmanifold; + btAlignedObjectArray 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 islandBodies; - //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - for (int startIslandIndex=0;startIslandIndexisActive()) 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 (startManifoldIndexProcessIsland(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 -#include @@ -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 +#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 m_elements; + btAlignedObjectArray 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; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp index b5f80de4557..636b0046c13 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -42,17 +42,16 @@ void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabb void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { - //float margin = 0.f; + //btScalar margin = btScalar(0.); btVector3 halfExtents = getHalfExtents(); - btScalar lx=2.f*(halfExtents.x()); - btScalar ly=2.f*(halfExtents.y()); - btScalar lz=2.f*(halfExtents.z()); - - inertia[0] = mass/(12.0f) * (ly*ly + lz*lz); - inertia[1] = mass/(12.0f) * (lx*lx + lz*lz); - inertia[2] = mass/(12.0f) * (lx*lx + ly*ly); + btScalar lx=btScalar(2.)*(halfExtents.x()); + btScalar ly=btScalar(2.)*(halfExtents.y()); + btScalar lz=btScalar(2.)*(halfExtents.z()); + inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), + mass/(btScalar(12.0)) * (lx*lx + lz*lz), + mass/(btScalar(12.0)) * (lx*lx + ly*ly)); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h index ee451a9b6c0..bc42f146c7c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -17,10 +17,10 @@ subject to the following restrictions: #define OBB_BOX_MINKOWSKI_H #include "btPolyhedralConvexShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "LinearMath/btPoint3.h" -#include "LinearMath/btSimdMinMax.h" +#include "btCollisionMargin.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" +#include "../../LinearMath/btPoint3.h" +#include "../../LinearMath/btSimdMinMax.h" ///btBoxShape implements both a feature based (vertex/edge/plane) and implicit (getSupportingVertex) Box class btBoxShape: public btPolyhedralConvexShape @@ -41,9 +41,9 @@ public: btVector3 halfExtents = getHalfExtents(); btVector3 supVertex; - supVertex = btPoint3(vec.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), - vec.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), - vec.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + supVertex = btPoint3(vec.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), + vec.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), + vec.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); return supVertex; } @@ -54,9 +54,9 @@ public: btVector3 margin(getMargin(),getMargin(),getMargin()); halfExtents -= margin; - return btVector3(vec.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), - vec.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), - vec.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + return btVector3(vec.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), + vec.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), + vec.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); } virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const @@ -69,9 +69,9 @@ public: for (int i=0;ibuild(meshInterface); + btVector3 bvhAabbMin,bvhAabbMax; + meshInterface->calculateAabbBruteForce(bvhAabbMin,bvhAabbMax); + m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); #endif //DISABLE_BVH } +btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) +:btTriangleMeshShape(meshInterface),m_useQuantizedAabbCompression(useQuantizedAabbCompression) +{ + //construct bvh from meshInterface +#ifndef DISABLE_BVH + + m_bvh = new btOptimizedBvh(); + m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); + +#endif //DISABLE_BVH + +} + +void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax) +{ + m_bvh->refitPartial( m_meshInterface,aabbMin,aabbMax ); + + m_localAabbMin.setMin(aabbMin); + m_localAabbMax.setMax(aabbMax); +} + + +void btBvhTriangleMeshShape::refitTree() +{ + m_bvh->refit( m_meshInterface ); + + recalcLocalAabb(); +} + btBvhTriangleMeshShape::~btBvhTriangleMeshShape() { delete m_bvh; @@ -63,7 +95,7 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co { } - virtual void processNode(const btOptimizedBvhNode* node) + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { const unsigned char *vertexbase; int numverts; @@ -84,19 +116,21 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co indexstride, numfaces, indicestype, - node->m_subPart); + nodeSubPart); - int* gfxbase = (int*)(indexbase+node->m_triangleIndex*indexstride); - + int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j=2;j>=0;j--) { int graphicsindex = gfxbase[j]; + + #ifdef DEBUG_TRIANGLE_MESH printf("%d ,",graphicsindex); #endif //DEBUG_TRIANGLE_MESH - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); m_triangle[j] = btVector3( graphicsbase[0]*meshScaling.getX(), @@ -107,8 +141,8 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co #endif //DEBUG_TRIANGLE_MESH } - m_callback->processTriangle(m_triangle,node->m_subPart,node->m_triangleIndex); - m_meshInterface->unLockReadOnlyVertexBase(node->m_subPart); + m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } }; @@ -130,8 +164,10 @@ void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) { btTriangleMeshShape::setLocalScaling(scaling); delete m_bvh; + ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work m_bvh = new btOptimizedBvh(); - m_bvh->build(m_meshInterface); //rebuild the bvh... + m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax); + } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 59a27e8641a..4914d9f959c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -16,20 +16,26 @@ subject to the following restrictions: #ifndef BVH_TRIANGLE_MESH_SHAPE_H #define BVH_TRIANGLE_MESH_SHAPE_H -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" +#include "btTriangleMeshShape.h" +#include "btOptimizedBvh.h" ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. -class btBvhTriangleMeshShape : public btTriangleMeshShape +ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape { btOptimizedBvh* m_bvh; - - + bool m_useQuantizedAabbCompression; + bool m_pad[12];////need padding due to alignment + public: - btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface); + btBvhTriangleMeshShape() :btTriangleMeshShape(0) {}; + btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression); + + ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb + btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); + virtual ~btBvhTriangleMeshShape(); @@ -44,6 +50,10 @@ public: virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void refitTree(); + + ///for a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks + void partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax); //debugging virtual char* getName()const {return "BVHTRIANGLEMESH";} @@ -51,8 +61,15 @@ public: virtual void setLocalScaling(const btVector3& scaling); - - -}; + btOptimizedBvh* getOptimizedBvh() + { + return m_bvh; + } + bool usesQuantizedAabbCompression() const + { + return m_useQuantizedAabbCompression; + } +} +; #endif //BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp new file mode 100644 index 00000000000..b7e15172da2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -0,0 +1,146 @@ +/* +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 "btCapsuleShape.h" + +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "LinearMath/btQuaternion.h" + +btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) +{ + m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); +} + + + btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +{ + + btVector3 supVec(0,0,0); + + btScalar maxDot(btScalar(-1e30)); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + + btVector3 vtx; + btScalar newDot; + + btScalar radius = getRadius(); + + + { + btVector3 pos(0,getHalfHeight(),0); + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + { + btVector3 pos(0,-getHalfHeight(),0); + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + + return supVec; + +} + + void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + + + btScalar radius = getRadius(); + + for (int j=0;j maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = vtx; + } + } + { + btVector3 pos(0,-getHalfHeight(),0); + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = vtx; + } + } + + } +} + + +void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + //as an approximation, take the inertia of the box that bounds the spheres + + btTransform ident; + ident.setIdentity(); + + + btScalar radius = getRadius(); + + btVector3 halfExtents(radius,radius+getHalfHeight(),radius); + + btScalar margin = CONVEX_DISTANCE_MARGIN; + + btScalar lx=btScalar(2.)*(halfExtents[0]+margin); + btScalar ly=btScalar(2.)*(halfExtents[1]+margin); + btScalar lz=btScalar(2.)*(halfExtents[2]+margin); + const btScalar x2 = lx*lx; + const btScalar y2 = ly*ly; + const btScalar z2 = lz*lz; + const btScalar scaledmass = mass * btScalar(.08333333); + + inertia[0] = scaledmass * (y2+z2); + inertia[1] = scaledmass * (x2+z2); + inertia[2] = scaledmass * (x2+y2); + +} + + + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h new file mode 100644 index 00000000000..27da8adefa5 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.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_CAPSULE_SHAPE_H +#define BT_CAPSULE_SHAPE_H + +#include "btConvexShape.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types + + +///btCapsuleShape represents a capsule around the Y axis +///A more general solution that can represent capsules is the btMultiSphereShape +class btCapsuleShape : public btConvexShape +{ + +public: + btCapsuleShape(btScalar radius,btScalar height); + + ///CollisionShape Interface + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + /// btConvexShape Interface + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + virtual int getShapeType() const { return CAPSULE_SHAPE_PROXYTYPE; } + + virtual char* getName()const + { + return "CapsuleShape"; + } + + btScalar getRadius() const + { + return m_implicitShapeDimensions.getX(); + } + + btScalar getHalfHeight() const + { + return m_implicitShapeDimensions.getY(); + } + +}; + + + +#endif //BT_CAPSULE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h index 377f0e506a2..4730264d3df 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h @@ -18,7 +18,7 @@ subject to the following restrictions: //used by Gjk and some other algorithms -#define CONVEX_DISTANCE_MARGIN 0.04f// 0.1f//;//0.01f +#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp index 5474a201c37..81d82428f4c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -15,6 +15,16 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCollisionShape.h" + +/* + Make sure this dummy function never changes so that it + can be used by probes that are checking whether the + library is actually installed. +*/ +extern "C" void btBulletCollisionProbe () {} + + + void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const { btTransform tr; @@ -23,14 +33,14 @@ void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) con getAabb(tr,aabbMin,aabbMax); - radius = (aabbMax-aabbMin).length()*0.5f; - center = (aabbMin+aabbMax)*0.5f; + radius = (aabbMax-aabbMin).length()*btScalar(0.5); + center = (aabbMin+aabbMax)*btScalar(0.5); } -float btCollisionShape::getAngularMotionDisc() const +btScalar btCollisionShape::getAngularMotionDisc() const { btVector3 center; - float disc; + btScalar disc; getBoundingSphere(center,disc); disc += (center).length(); return disc; @@ -41,25 +51,25 @@ void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const b //start with static aabb getAabb(curTrans,temporalAabbMin,temporalAabbMax); - float temporalAabbMaxx = temporalAabbMax.getX(); - float temporalAabbMaxy = temporalAabbMax.getY(); - float temporalAabbMaxz = temporalAabbMax.getZ(); - float temporalAabbMinx = temporalAabbMin.getX(); - float temporalAabbMiny = temporalAabbMin.getY(); - float temporalAabbMinz = temporalAabbMin.getZ(); + btScalar temporalAabbMaxx = temporalAabbMax.getX(); + btScalar temporalAabbMaxy = temporalAabbMax.getY(); + btScalar temporalAabbMaxz = temporalAabbMax.getZ(); + btScalar temporalAabbMinx = temporalAabbMin.getX(); + btScalar temporalAabbMiny = temporalAabbMin.getY(); + btScalar temporalAabbMinz = temporalAabbMin.getZ(); // add linear motion btVector3 linMotion = linvel*timeStep; //todo: simd would have a vector max/min operation, instead of per-element access - if (linMotion.x() > 0.f) + if (linMotion.x() > btScalar(0.)) temporalAabbMaxx += linMotion.x(); else temporalAabbMinx += linMotion.x(); - if (linMotion.y() > 0.f) + if (linMotion.y() > btScalar(0.)) temporalAabbMaxy += linMotion.y(); else temporalAabbMiny += linMotion.y(); - if (linMotion.z() > 0.f) + if (linMotion.z() > btScalar(0.)) temporalAabbMaxz += linMotion.z(); else temporalAabbMinz += linMotion.z(); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index 7b2a00a1c57..96268734a83 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -16,18 +16,18 @@ subject to the following restrictions: #ifndef COLLISION_SHAPE_H #define COLLISION_SHAPE_H -#include "LinearMath/btTransform.h" -#include "LinearMath/btVector3.h" -#include -#include "LinearMath/btPoint3.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "../../LinearMath/btPoint3.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" //for the shape types ///btCollisionShape provides interface for collision shapes that can be shared among btCollisionObjects. class btCollisionShape { public: - btCollisionShape() :m_tempDebug(0) + btCollisionShape() { } virtual ~btCollisionShape() @@ -40,14 +40,15 @@ public: virtual void getBoundingSphere(btVector3& center,btScalar& radius) const; ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. - virtual float getAngularMotionDisc() const; + virtual btScalar getAngularMotionDisc() const; - virtual int getShapeType() const=0; ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax); +#ifndef __SPU__ + inline bool isPolyhedral() const { return btBroadphaseProxy::isPolyhedral(getShapeType()); @@ -72,20 +73,20 @@ public: return btBroadphaseProxy::isInfinite(getShapeType()); } + virtual int getShapeType() const=0; virtual void setLocalScaling(const btVector3& scaling) =0; virtual const btVector3& getLocalScaling() const =0; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) = 0; + //debugging support virtual char* getName()const =0 ; - const char* getExtraDebugInfo() const { return m_tempDebug;} - void setExtraDebugInfo(const char* extraDebugInfo) { m_tempDebug = extraDebugInfo;} - const char * m_tempDebug; -//endif debugging support +#endif //__SPU__ + + - virtual void setMargin(float margin) = 0; - virtual float getMargin() const = 0; + virtual void setMargin(btScalar margin) = 0; + virtual btScalar getMargin() const = 0; }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 88ae8c7dfd4..a4712b3e925 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -20,11 +20,11 @@ subject to the following restrictions: btCompoundShape::btCompoundShape() -:m_localAabbMin(1e30f,1e30f,1e30f), -m_localAabbMax(-1e30f,-1e30f,-1e30f), +:m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)), +m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)), m_aabbTree(0), -m_collisionMargin(0.f), -m_localScaling(1.f,1.f,1.f) +m_collisionMargin(btScalar(0.)), +m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) { } @@ -60,8 +60,8 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const { - btVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin); - btVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin); + btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); btMatrix3x3 abs_b = trans.getBasis().absolute(); @@ -84,15 +84,15 @@ void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) btVector3 aabbMin,aabbMax; getAabb(ident,aabbMin,aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); - btScalar lx=2.f*(halfExtents.x()); - btScalar ly=2.f*(halfExtents.y()); - btScalar lz=2.f*(halfExtents.z()); + btScalar lx=btScalar(2.)*(halfExtents.x()); + btScalar ly=btScalar(2.)*(halfExtents.y()); + btScalar lz=btScalar(2.)*(halfExtents.z()); - inertia[0] = mass/(12.0f) * (ly*ly + lz*lz); - inertia[1] = mass/(12.0f) * (lx*lx + lz*lz); - inertia[2] = mass/(12.0f) * (lx*lx + ly*ly); + inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz); + inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz); + inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 84188bc8b76..2d384bd2180 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -18,12 +18,11 @@ subject to the following restrictions: #include "btCollisionShape.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btMatrix3x3.h" -#include -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "LinearMath/btAlignedObjectArray.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "btCollisionMargin.h" +#include "../../LinearMath/btAlignedObjectArray.h" class btOptimizedBvh; @@ -85,11 +84,11 @@ public: virtual int getShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;} - virtual void setMargin(float margin) + virtual void setMargin(btScalar margin) { m_collisionMargin = margin; } - virtual float getMargin() const + virtual btScalar getMargin() const { return m_collisionMargin; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp index 7cd35a91860..5103500a012 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp @@ -17,7 +17,7 @@ subject to the following restrictions: #include "btConcaveShape.h" -btConcaveShape::btConcaveShape() : m_collisionMargin(0.f) +btConcaveShape::btConcaveShape() : m_collisionMargin(btScalar(0.)) { } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h index 2bbba88bc44..73f974e4ee9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef CONCAVE_SHAPE_H #define CONCAVE_SHAPE_H -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types - +#include "btCollisionShape.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "btTriangleCallback.h" @@ -27,7 +26,7 @@ subject to the following restrictions: class btConcaveShape : public btCollisionShape { protected: - float m_collisionMargin; + btScalar m_collisionMargin; public: btConcaveShape(); @@ -36,10 +35,10 @@ public: virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const = 0; - virtual float getMargin() const { + virtual btScalar getMargin() const { return m_collisionMargin; } - virtual void setMargin(float collisionMargin) + virtual void setMargin(btScalar collisionMargin) { m_collisionMargin = collisionMargin; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp index 46232b3187c..207b3024bc3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -24,7 +24,7 @@ m_height(height) { setConeUpIndex(1); btVector3 halfExtents; - m_sinAngle = (m_radius / sqrt(m_radius * m_radius + m_height * m_height)); + m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height)); } btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height): @@ -67,15 +67,15 @@ void btConeShape::setConeUpIndex(int upIndex) btVector3 btConeShape::coneLocalSupport(const btVector3& v) const { - float halfHeight = m_height * 0.5f; + btScalar halfHeight = m_height * btScalar(0.5); if (v[m_coneIndices[1]] > v.length() * m_sinAngle) { btVector3 tmp; - tmp[m_coneIndices[0]] = 0.f; + tmp[m_coneIndices[0]] = btScalar(0.); tmp[m_coneIndices[1]] = halfHeight; - tmp[m_coneIndices[2]] = 0.f; + tmp[m_coneIndices[2]] = btScalar(0.); return tmp; } else { @@ -90,9 +90,9 @@ btVector3 btConeShape::coneLocalSupport(const btVector3& v) const } else { btVector3 tmp; - tmp[m_coneIndices[0]] = 0.f; + tmp[m_coneIndices[0]] = btScalar(0.); tmp[m_coneIndices[1]] = -halfHeight; - tmp[m_coneIndices[2]] = 0.f; + tmp[m_coneIndices[2]] = btScalar(0.); return tmp; } } @@ -117,12 +117,12 @@ void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVect btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = coneLocalSupport(vec); - if ( getMargin()!=0.f ) + if ( getMargin()!=btScalar(0.) ) { btVector3 vecnorm = vec; if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) { - vecnorm.setValue(-1.f,-1.f,-1.f); + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); } vecnorm.normalize(); supVertex+= getMargin() * vecnorm; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index 52d925fe714..3ccda5b12c6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -17,16 +17,16 @@ subject to the following restrictions: #define CONE_MINKOWSKI_H #include "btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types ///btConeShape implements a Cone shape, around the Y axis class btConeShape : public btConvexShape { - float m_sinAngle; - float m_radius; - float m_height; + btScalar m_sinAngle; + btScalar m_radius; + btScalar m_height; int m_coneIndices[3]; btVector3 coneLocalSupport(const btVector3& v) const; @@ -38,8 +38,8 @@ public: virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - float getRadius() const { return m_radius;} - float getHeight() const { return m_height;} + btScalar getRadius() const { return m_radius;} + btScalar getHeight() const { return m_height;} virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) @@ -49,17 +49,17 @@ public: btVector3 aabbMin,aabbMax; getAabb(identity,aabbMin,aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); - float margin = getMargin(); + btScalar margin = getMargin(); - btScalar lx=2.f*(halfExtents.x()+margin); - btScalar ly=2.f*(halfExtents.y()+margin); - btScalar lz=2.f*(halfExtents.z()+margin); + btScalar lx=btScalar(2.)*(halfExtents.x()+margin); + btScalar ly=btScalar(2.)*(halfExtents.y()+margin); + btScalar lz=btScalar(2.)*(halfExtents.z()+margin); const btScalar x2 = lx*lx; const btScalar y2 = ly*ly; const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * 0.08333333f; + const btScalar scaledmass = mass * btScalar(0.08333333); inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 5af6e5f03d9..deb3954b5c3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: -btConvexHullShape ::btConvexHullShape (const float* points,int numPoints,int stride) +btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) { m_points.resize(numPoints); @@ -30,26 +30,37 @@ btConvexHullShape ::btConvexHullShape (const float* points,int numPoints,int str btPoint3* point = (btPoint3*)(pointsBaseAddress + i*stride); m_points[i] = point[0]; } + + recalcLocalAabb(); + +} + + +void btConvexHullShape::addPoint(const btPoint3& point) +{ + m_points.push_back(point); + recalcLocalAabb(); + } btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const { - btVector3 supVec(0.f,0.f,0.f); - btScalar newDot,maxDot = -1e30f; + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar newDot,maxDot = btScalar(-1e30); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < 0.0001f) + if (lenSqr < btScalar(0.0001)) { vec.setValue(1,0,0); } else { - float rlen = 1.f / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); vec *= rlen; } - for (size_t i=0;i m_points; public: - ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive float (x,y,z), the striding defines the number of bytes between each point, in memory. + + + ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive btScalar (x,y,z), the striding defines the number of bytes between each point, in memory. ///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint. ///btConvexHullShape make an internal copy of the points. - btConvexHullShape(const float* points=0,int numPoints=0, int stride=sizeof(btPoint3)); + btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btPoint3)); + + void addPoint(const btPoint3& point); - void addPoint(const btPoint3& point) + btPoint3* getPoints() { - m_points.push_back(point); + return &m_points[0]; } + + int getNumPoints() + { + return m_points.size(); + } + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 9537235ff8a..7edf1ea6db8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -15,8 +15,9 @@ subject to the following restrictions: #include "btConvexShape.h" + btConvexShape::btConvexShape() -: m_localScaling(1.f,1.f,1.f), +: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), m_collisionMargin(CONVEX_DISTANCE_MARGIN) { } @@ -35,35 +36,42 @@ void btConvexShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVec btScalar margin = getMargin(); for (int i=0;i<3;i++) { - btVector3 vec(0.f,0.f,0.f); - vec[i] = 1.f; + btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + vec[i] = btScalar(1.); btVector3 sv = localGetSupportingVertex(vec*trans.getBasis()); btVector3 tmp = trans(sv); maxAabb[i] = tmp[i]+margin; - vec[i] = -1.f; + vec[i] = btScalar(-1.); tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); minAabb[i] = tmp[i]-margin; } }; + btVector3 btConvexShape::localGetSupportingVertex(const btVector3& vec)const - { +{ +#ifndef __SPU__ + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=0.f ) + if ( getMargin()!=btScalar(0.) ) { btVector3 vecnorm = vec; if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) { - vecnorm.setValue(-1.f,-1.f,-1.f); + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); } vecnorm.normalize(); supVertex+= getMargin() * vecnorm; } return supVertex; +#else + return btVector3(0,0,0); +#endif //__SPU__ + } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h index 0de334b3521..746f383dfc7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -18,11 +18,10 @@ subject to the following restrictions: #include "btCollisionShape.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btMatrix3x3.h" -#include -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "btCollisionMargin.h" //todo: get rid of this btConvexCastResult thing! struct btConvexCastResult; @@ -31,9 +30,9 @@ struct btConvexCastResult; /// btConvexShape is an abstract shape interface. /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. /// used in combination with GJK or btConvexCast -class btConvexShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) btConvexShape : public btCollisionShape { - + protected: //local scaling. collisionMargin is not scaled ! @@ -43,14 +42,27 @@ protected: btScalar m_collisionMargin; + btScalar m_padding[2]; + + + + public: btConvexShape(); + virtual ~btConvexShape() + { + + } + + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; +#ifndef __SPU__ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0; //notice that the vectors should be unit length virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; +#endif //#ifndef __SPU__ const btVector3& getImplicitShapeDimensions() const { @@ -74,12 +86,21 @@ public: return m_localScaling; } + const btVector3& getLocalScalingNV() const + { + return m_localScaling; + } - virtual void setMargin(float margin) + virtual void setMargin(btScalar margin) { m_collisionMargin = margin; } - virtual float getMargin() const + virtual btScalar getMargin() const + { + return m_collisionMargin; + } + + btScalar getMarginNV() const { return m_collisionMargin; } @@ -91,12 +112,15 @@ public: virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { - assert(0); + (void)penetrationVector; + (void)index; + btAssert(0); } -}; +} +; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index 9d8e7e1408e..614ec977793 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -22,6 +22,7 @@ subject to the following restrictions: btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface) :m_stridingMesh(meshInterface) { + recalcLocalAabb(); } @@ -39,14 +40,17 @@ public: btVector3 m_supportVecLocal; LocalSupportVertexCallback(const btVector3& supportVecLocal) - : m_supportVertexLocal(0.f,0.f,0.f), - m_maxDot(-1e30f), + : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), + m_maxDot(btScalar(-1e30)), m_supportVecLocal(supportVecLocal) { } virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) { + (void)triangleIndex; + (void)partId; + for (int i=0;i<3;i++) { btScalar dot = m_supportVecLocal.dot(triangle[i]); @@ -71,21 +75,21 @@ public: btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const { - btVector3 supVec(0.f,0.f,0.f); + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < 0.0001f) + if (lenSqr < btScalar(0.0001)) { vec.setValue(1,0,0); } else { - float rlen = 1.f / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); vec *= rlen; } LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(1e30f,1e30f,1e30f); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); supVec = supportCallback.GetSupportVertexLocal(); @@ -98,7 +102,7 @@ void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargi { for (int i=0;iInternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); } @@ -122,12 +126,12 @@ btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& v { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=0.f ) + if ( getMargin()!=btScalar(0.) ) { btVector3 vecnorm = vec; if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) { - vecnorm.setValue(-1.f,-1.f,-1.f); + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); } vecnorm.normalize(); supVertex+= getMargin() * vecnorm; @@ -157,14 +161,14 @@ int btConvexTriangleMeshShape::getNumEdges() const return 0; } -void btConvexTriangleMeshShape::getEdge(int i,btPoint3& pa,btPoint3& pb) const +void btConvexTriangleMeshShape::getEdge(int ,btPoint3& ,btPoint3& ) const { - assert(0); + btAssert(0); } -void btConvexTriangleMeshShape::getVertex(int i,btPoint3& vtx) const +void btConvexTriangleMeshShape::getVertex(int ,btPoint3& ) const { - assert(0); + btAssert(0); } int btConvexTriangleMeshShape::getNumPlanes() const @@ -172,15 +176,15 @@ int btConvexTriangleMeshShape::getNumPlanes() const return 0; } -void btConvexTriangleMeshShape::getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const +void btConvexTriangleMeshShape::getPlane(btVector3& ,btPoint3& ,int ) const { - assert(0); + btAssert(0); } //not yet -bool btConvexTriangleMeshShape::isInside(const btPoint3& pt,btScalar tolerance) const +bool btConvexTriangleMeshShape::isInside(const btPoint3& ,btScalar ) const { - assert(0); + btAssert(0); return false; } @@ -189,5 +193,13 @@ bool btConvexTriangleMeshShape::isInside(const btPoint3& pt,btScalar tolerance) void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_stridingMesh->setScaling(scaling); + + recalcLocalAabb(); + } + +const btVector3& btConvexTriangleMeshShape::getLocalScaling() const +{ + return m_stridingMesh->getScaling(); +} \ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h index 86e871c6c0b..34ee7af744c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -3,9 +3,8 @@ #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types -#include /// btConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use btConvexHullShape instead. /// It uses the btStridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data. @@ -40,7 +39,8 @@ public: virtual bool isInside(const btPoint3& pt,btScalar tolerance) const; - void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 16b263474f0..1666afb3b88 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -16,23 +16,33 @@ subject to the following restrictions: #include "LinearMath/btPoint3.h" btCylinderShape::btCylinderShape (const btVector3& halfExtents) -:btBoxShape(halfExtents) +:btBoxShape(halfExtents), +m_upAxis(1) { - + recalcLocalAabb(); } btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents) :btCylinderShape(halfExtents) { + m_upAxis = 0; + recalcLocalAabb(); } btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents) :btCylinderShape(halfExtents) { + m_upAxis = 2; + recalcLocalAabb(); } +void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + //skip the box 'getAabb' + btPolyhedralConvexShape::getAabb(t,aabbMin,aabbMax); +} inline btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) @@ -46,8 +56,8 @@ const int ZZ = 2; // extents of the cylinder is: X,Y is for radius, and Z for height - float radius = halfExtents[XX]; - float halfHeight = halfExtents[cylinderUpAxis]; + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; btVector3 tmp; @@ -87,8 +97,8 @@ const int YY = 1; const int ZZ = 2; - float radius = halfExtents[XX]; - float halfHeight = halfExtents[cylinderUpAxis]; + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; btVector3 tmp; @@ -124,8 +134,8 @@ const int ZZ = 1; // extents of the cylinder is: X,Y is for radius, and Z for height - float radius = halfExtents[XX]; - float halfHeight = halfExtents[cylinderUpAxis]; + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; btVector3 tmp; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h index bb0e4298abc..c78890c887a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -17,22 +17,23 @@ subject to the following restrictions: #define CYLINDER_MINKOWSKI_H #include "btBoxShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types -#include "LinearMath/btVector3.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../../LinearMath/btVector3.h" /// implements cylinder shape interface class btCylinderShape : public btBoxShape { +protected: + + int m_upAxis; + public: btCylinderShape (const btVector3& halfExtents); ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - getAabbSlow(t,aabbMin,aabbMax); - } + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; @@ -44,12 +45,12 @@ public: btVector3 supVertex; supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=0.f ) + if ( getMargin()!=btScalar(0.) ) { btVector3 vecnorm = vec; if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) { - vecnorm.setValue(-1.f,-1.f,-1.f); + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); } vecnorm.normalize(); supVertex+= getMargin() * vecnorm; @@ -66,12 +67,12 @@ public: return CYLINDER_SHAPE_PROXYTYPE; } - virtual int getUpAxis() const + int getUpAxis() const { - return 1; + return m_upAxis; } - virtual float getRadius() const + virtual btScalar getRadius() const { return getHalfExtents().getX(); } @@ -93,17 +94,14 @@ public: virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual int getUpAxis() const - { - return 0; - } + //debugging virtual char* getName()const { return "CylinderX"; } - virtual float getRadius() const + virtual btScalar getRadius() const { return getHalfExtents().getY(); } @@ -128,7 +126,7 @@ public: return "CylinderZ"; } - virtual float getRadius() const + virtual btScalar getRadius() const { return getHalfExtents().getX(); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp index c63f4a9c571..dcc486f194b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp @@ -40,9 +40,9 @@ void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aa } -void btEmptyShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) { - assert(0); + btAssert(0); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h index c16149e55fb..8ec53444976 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h @@ -18,11 +18,10 @@ subject to the following restrictions: #include "btConcaveShape.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btMatrix3x3.h" -#include -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "btCollisionMargin.h" diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 57132ee9512..5a359c0cff1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: btHeightfieldTerrainShape::btHeightfieldTerrainShape() -:m_localScaling(0.f,0.f,0.f) +:m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.)) { } @@ -30,11 +30,10 @@ btHeightfieldTerrainShape::~btHeightfieldTerrainShape() -void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btHeightfieldTerrainShape::getAabb(const btTransform& ,btVector3& aabbMin,btVector3& aabbMax) const { - aabbMin.setValue(-1e30f,-1e30f,-1e30f); - aabbMax.setValue(1e30f,1e30f,1e30f); - + aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); } @@ -42,14 +41,17 @@ void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin, void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { + (void)callback; + (void)aabbMax; + (void)aabbMin; - btVector3 halfExtents = (aabbMax - aabbMin) * 0.5f; - btScalar radius = halfExtents.length(); - btVector3 center = (aabbMax + aabbMin) * 0.5f; + /* + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); + btVector3 center = (aabbMax + aabbMin) * btScalar(0.5); //TODO //this is where the triangles are generated, given AABB and plane equation (normal/constant) -/* + btVector3 tangentDir0,tangentDir1; //tangentDir0/tangentDir1 can be precalculated @@ -60,6 +62,7 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal; btVector3 triangle[3]; + btScalar radius = halfExtents.length(); triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius; triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius; triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius; @@ -75,11 +78,11 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback } -void btHeightfieldTerrainShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) { //moving concave objects not supported - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index e4cc9d42d59..85fd49638ce 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef HEIGHTFIELD_TERRAIN_SHAPE_H #define HEIGHTFIELD_TERRAIN_SHAPE_H -#include "BulletCollision/CollisionShapes/btConcaveShape.h" +#include "btConcaveShape.h" ///btHeightfieldTerrainShape simulates a 2D heightfield terrain class btHeightfieldTerrainShape : public btConcaveShape diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 4bf8c478a53..b213f3819a4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -43,7 +43,7 @@ void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(cons -float btMinkowskiSumShape::getMargin() const +btScalar btMinkowskiSumShape::getMargin() const { return m_shapeA->getMargin() + m_shapeB->getMargin(); } @@ -51,6 +51,7 @@ float btMinkowskiSumShape::getMargin() const void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { - assert(0); + (void)mass; + btAssert(0); inertia.setValue(0,0,0); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index e974f57a4c2..489b7ddde1e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define MINKOWSKI_SUM_SHAPE_H #include "btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "../BroadphaseCollision/btBroadphaseProxy.h" // for the types /// btMinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. class btMinkowskiSumShape : public btConvexShape @@ -48,7 +48,7 @@ public: virtual int getShapeType() const { return MINKOWSKI_SUM_SHAPE_PROXYTYPE; } - virtual float getMargin() const; + virtual btScalar getMargin() const; const btConvexShape* getShapeA() const { return m_shapeA;} const btConvexShape* getShapeB() const { return m_shapeB;} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index fde4d95da14..6015a618082 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres) :m_inertiaHalfExtents(inertiaHalfExtents) { - float startMargin = 1e30f; + btScalar startMargin = btScalar(1e30); m_numSpheres = numSpheres; for (int i=0;i=0); + + btQuantizedBvhNode node; + btVector3 aabbMin,aabbMax; + aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMin.setMin(triangle[0]); + aabbMax.setMax(triangle[0]); + aabbMin.setMin(triangle[1]); + aabbMax.setMax(triangle[1]); + aabbMin.setMin(triangle[2]); + aabbMax.setMax(triangle[2]); + + m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMin[0],aabbMin); + m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMax[0],aabbMax); + + node.m_escapeIndexOrTriangleIndex = triangleIndex; - m_triangleNodes.push_back(node); } }; + + + + int numLeafNodes = 0; + if (m_useQuantization) + { - NodeTriangleCallback callback(m_leafNodes); + //initialize quantization values + setQuantizationValues(bvhAabbMin,bvhAabbMax); + + QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this); + + + triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax); - btVector3 aabbMin(-1e30f,-1e30f,-1e30f); - btVector3 aabbMax(1e30f,1e30f,1e30f); + //now we have an array of leafnodes in m_leafNodes + numLeafNodes = m_quantizedLeafNodes.size(); - triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); - //now we have an array of leafnodes in m_leafNodes + m_quantizedContiguousNodes.resize(2*numLeafNodes); + + + } else + { + NodeTriangleCallback callback(m_leafNodes); + + btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + + triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); + + //now we have an array of leafnodes in m_leafNodes + numLeafNodes = m_leafNodes.size(); + + m_contiguousNodes.resize(2*numLeafNodes); + } - m_contiguousNodes = new btOptimizedBvhNode[2*m_leafNodes.size()]; m_curNodeIndex = 0; - m_rootNode1 = buildTree(m_leafNodes,0,m_leafNodes.size()); + buildTree(0,numLeafNodes); + + ///if the entire tree is small then subtree size, we need to create a header info for the tree + if(m_useQuantization && !m_SubtreeHeaders.size()) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); + subtree.m_rootNodeIndex = 0; + subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); + } +} + + + +void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) +{ + //incrementally initialize quantization values + btAssert(m_useQuantization); + + btAssert(aabbMin.getX() > m_bvhAabbMin.getX()); + btAssert(aabbMin.getY() > m_bvhAabbMin.getY()); + btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ()); + + btAssert(aabbMax.getX() < m_bvhAabbMax.getX()); + btAssert(aabbMax.getY() < m_bvhAabbMax.getY()); + btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ()); + + ///we should update all quantization values, using updateBvhNodes(meshInterface); + ///but we only update chunks that overlap the given aabb + + unsigned short quantizedQueryAabbMin[3]; + unsigned short quantizedQueryAabbMax[3]; + + quantizeWithClamp(&quantizedQueryAabbMin[0],aabbMin); + quantizeWithClamp(&quantizedQueryAabbMax[0],aabbMax); + int i; + for (i=0;im_SubtreeHeaders.size();i++) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; - ///create the leafnodes first -// btOptimizedBvhNode* leafNodes = new btOptimizedBvhNode; + bool overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap) + { + updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i); + + subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); + } + } + } +///just for debugging, to visualize the individual patches/subtrees +#ifdef DEBUG_PATCH_COLORS +btVector3 color[4]= +{ + btVector3(255,0,0), + btVector3(0,255,0), + btVector3(0,0,255), + btVector3(0,255,255) +}; +#endif //DEBUG_PATCH_COLORS + + +void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index) +{ + (void)index; + + btAssert(m_useQuantization); + + int nodeSubPart=0; + + //get access info to trianglemesh data + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart); + + btVector3 triangleVerts[3]; + btVector3 aabbMin,aabbMax; + const btVector3& meshScaling = meshInterface->getScaling(); + + int i; + for (i=endNode-1;i>=firstNode;i--) + { + + + btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; + if (curNode.isLeafNode()) + { + //recalc aabb from triangle data + int nodeTriangleIndex = curNode.getTriangleIndex(); + //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, + + int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + + + for (int j=2;j>=0;j--) + { + + int graphicsindex = gfxbase[j]; + btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); +#ifdef DEBUG_PATCH_COLORS + btVector3 mycolor = color[index&3]; + graphicsbase[8] = mycolor.getX(); + graphicsbase[9] = mycolor.getY(); + graphicsbase[10] = mycolor.getZ(); +#endif //DEBUG_PATCH_COLORS + + + triangleVerts[j] = btVector3( + graphicsbase[0]*meshScaling.getX(), + graphicsbase[1]*meshScaling.getY(), + graphicsbase[2]*meshScaling.getZ()); + } + + + + aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMin.setMin(triangleVerts[0]); + aabbMax.setMax(triangleVerts[0]); + aabbMin.setMin(triangleVerts[1]); + aabbMax.setMax(triangleVerts[1]); + aabbMin.setMin(triangleVerts[2]); + aabbMax.setMax(triangleVerts[2]); + + quantizeWithClamp(&curNode.m_quantizedAabbMin[0],aabbMin); + quantizeWithClamp(&curNode.m_quantizedAabbMax[0],aabbMax); + + } else + { + //combine aabb from both children + + btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1]; + + btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] : + &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()]; + + + { + for (int i=0;i<3;i++) + { + curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i]; + if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i]) + curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i]; + + curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i]; + if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i]) + curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i]; + } + } + } + + } + + meshInterface->unLockReadOnlyVertexBase(nodeSubPart); + + +} + +void btOptimizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin) +{ + //enlarge the AABB to avoid division by zero when initializing the quantization values + btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); + m_bvhAabbMin = bvhAabbMin - clampValue; + m_bvhAabbMax = bvhAabbMax + clampValue; + btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin; + m_bvhQuantization = btVector3(btScalar(65535.0),btScalar(65535.0),btScalar(65535.0)) / aabbSize; +} + + +void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface) +{ + if (m_useQuantization) + { + //calculate new aabb + btVector3 aabbMin,aabbMax; + meshInterface->calculateAabbBruteForce(aabbMin,aabbMax); + + setQuantizationValues(aabbMin,aabbMax); + + updateBvhNodes(meshInterface,0,m_curNodeIndex,0); + + ///now update all subtree headers + + int i; + for (i=0;i gMaxStackDepth) + gMaxStackDepth = gStackDepth; +#endif //DEBUG_TREE_BUILDING + int splitAxis, splitIndex, i; int numIndices =endIndex-startIndex; @@ -108,96 +392,172 @@ btOptimizedBvhNode* btOptimizedBvh::buildTree (NodeArray& leafNodes,int startInd if (numIndices==1) { - return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]); +#ifdef DEBUG_TREE_BUILDING + gStackDepth--; +#endif //DEBUG_TREE_BUILDING + + assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex); + + m_curNodeIndex++; + return; } //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. - splitAxis = calcSplittingAxis(leafNodes,startIndex,endIndex); + splitAxis = calcSplittingAxis(startIndex,endIndex); - splitIndex = sortAndCalcSplittingIndex(leafNodes,startIndex,endIndex,splitAxis); + splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis); - internalNode = &m_contiguousNodes[m_curNodeIndex++]; + int internalNodeIndex = m_curNodeIndex; - internalNode->m_aabbMax.setValue(-1e30f,-1e30f,-1e30f); - internalNode->m_aabbMin.setValue(1e30f,1e30f,1e30f); + setInternalNodeAabbMax(m_curNodeIndex,btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30))); + setInternalNodeAabbMin(m_curNodeIndex,btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30))); for (i=startIndex;im_aabbMax.setMax(leafNodes[i].m_aabbMax); - internalNode->m_aabbMin.setMin(leafNodes[i].m_aabbMin); + mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i)); } + m_curNodeIndex++; //internalNode->m_escapeIndex; - internalNode->m_leftChild = buildTree(leafNodes,startIndex,splitIndex); - internalNode->m_rightChild = buildTree(leafNodes,splitIndex,endIndex); + + int leftChildNodexIndex = m_curNodeIndex; + + //build left child tree + buildTree(startIndex,splitIndex); + + int rightChildNodexIndex = m_curNodeIndex; + //build right child tree + buildTree(splitIndex,endIndex); + +#ifdef DEBUG_TREE_BUILDING + gStackDepth--; +#endif //DEBUG_TREE_BUILDING + + int escapeIndex = m_curNodeIndex - curIndex; + + if (m_useQuantization) + { + //escapeIndex is the number of nodes of this subtree + const int sizeQuantizedNode =sizeof(btQuantizedBvhNode); + const int treeSizeInBytes = escapeIndex * sizeQuantizedNode; + if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) + { + updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex); + } + } + + setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex); - internalNode->m_escapeIndex = m_curNodeIndex - curIndex; - return internalNode; } -int btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis) +void btOptimizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex) +{ + btAssert(m_useQuantization); + + btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex]; + int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex(); + int leftSubTreeSizeInBytes = leftSubTreeSize * sizeof(btQuantizedBvhNode); + + btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex]; + int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex(); + int rightSubTreeSizeInBytes = rightSubTreeSize * sizeof(btQuantizedBvhNode); + + if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(leftChildNode); + subtree.m_rootNodeIndex = leftChildNodexIndex; + subtree.m_subtreeSize = leftSubTreeSize; + } + + if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(rightChildNode); + subtree.m_rootNodeIndex = rightChildNodexIndex; + subtree.m_subtreeSize = rightSubTreeSize; + } +} + + +int btOptimizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis) { int i; int splitIndex =startIndex; int numIndices = endIndex - startIndex; - float splitValue; + btScalar splitValue; - btVector3 means(0.f,0.f,0.f); + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); for (i=startIndex;i splitValue) { //swap - btOptimizedBvhNode tmp = leafNodes[i]; - leafNodes[i] = leafNodes[splitIndex]; - leafNodes[splitIndex] = tmp; + swapLeafNodes(i,splitIndex); splitIndex++; } } - if ((splitIndex==startIndex) || (splitIndex == (endIndex-1))) + + //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex + //otherwise the tree-building might fail due to stack-overflows in certain cases. + //unbalanced1 is unsafe: it can cause stack overflows + //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); + + //unbalanced2 should work too: always use center (perfect balanced trees) + //bool unbalanced2 = true; + + //this should be safe too: + int rangeBalancedIndices = numIndices/3; + bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + + if (unbalanced) { splitIndex = startIndex+ (numIndices>>1); } + + bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); + btAssert(!unbal); + return splitIndex; } -int btOptimizedBvh::calcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex) +int btOptimizedBvh::calcSplittingAxis(int startIndex,int endIndex) { int i; - btVector3 means(0.f,0.f,0.f); - btVector3 variance(0.f,0.f,0.f); + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); int numIndices = endIndex-startIndex; for (i=startIndex;im_aabbMinOrg,rootNode->m_aabbMaxOrg); + isLeafNode = rootNode->m_escapeIndex == -1; + + if (isLeafNode && aabbOverlap) + { + nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); + } + + if (aabbOverlap || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->m_escapeIndex; + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + +/* +///this was the original recursive traversal, before we optimized towards stackless traversal void btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const { bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); @@ -230,27 +662,82 @@ void btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback } } +*/ -int maxIterations = 0; +void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + btAssert(m_useQuantization); + + bool aabbOverlap, isLeafNode; -void btOptimizedBvh::walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax); + isLeafNode = currentNode->isLeafNode(); + + if (aabbOverlap) + { + if (isLeafNode) + { + nodeCallback->processNode(0,currentNode->getTriangleIndex()); + } else + { + //process left and right children + const btQuantizedBvhNode* leftChildNode = currentNode+1; + walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + + const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex(); + walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + } + } +} + + + + + + + +void btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const { - int escapeIndex, curIndex = 0; + btAssert(m_useQuantization); + + int curIndex = startNodeIndex; int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + bool aabbOverlap, isLeafNode; - while (curIndex < m_curNodeIndex) + while (curIndex < endNodeIndex) { + +//#define VISUALLY_ANALYZE_BVH 1 +#ifdef VISUALLY_ANALYZE_BVH + //some code snippet to debugDraw aabb, to visually analyze bvh structure + static int drawPatch = 0; + //need some global access to a debugDrawer + extern btIDebugDraw* debugDrawerPtr; + if (curIndex==drawPatch) + { + btVector3 aabbMin,aabbMax; + aabbMin = unQuantize(rootNode->m_quantizedAabbMin); + aabbMax = unQuantize(rootNode->m_quantizedAabbMax); + btVector3 color(1,0,0); + debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + } +#endif//VISUALLY_ANALYZE_BVH + //catch bugs in tree data - assert (walkIterations < m_curNodeIndex); + assert (walkIterations < subTreeSize); walkIterations++; - aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); - isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild); + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); if (isLeafNode && aabbOverlap) { - nodeCallback->processNode(rootNode); + nodeCallback->processNode(0,rootNode->getTriangleIndex()); } if (aabbOverlap || isLeafNode) @@ -259,21 +746,100 @@ void btOptimizedBvh::walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverla curIndex++; } else { - escapeIndex = rootNode->m_escapeIndex; + escapeIndex = rootNode->getEscapeIndex(); rootNode += escapeIndex; curIndex += escapeIndex; } - } - if (maxIterations < walkIterations) maxIterations = walkIterations; } +//This traversal can be called from Playstation 3 SPU +void btOptimizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + btAssert(m_useQuantization); + + int i; + + + for (i=0;im_SubtreeHeaders.size();i++) + { + const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; + + bool overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap) + { + walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, + subtree.m_rootNodeIndex, + subtree.m_rootNodeIndex+subtree.m_subtreeSize); + } + } +} + + + void btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const { + (void)nodeCallback; + (void)aabbMin; + (void)aabbMax; + //not yet, please use aabb + btAssert(0); +} + + +void btOptimizedBvh::quantizeWithClamp(unsigned short* out, const btVector3& point) const +{ + + btAssert(m_useQuantization); + + btVector3 clampedPoint(point); + clampedPoint.setMax(m_bvhAabbMin); + clampedPoint.setMin(m_bvhAabbMax); + + btVector3 v = (clampedPoint - m_bvhAabbMin) * m_bvhQuantization; + out[0] = (unsigned short)(v.getX()+0.5f); + out[1] = (unsigned short)(v.getY()+0.5f); + out[2] = (unsigned short)(v.getZ()+0.5f); +} + +btVector3 btOptimizedBvh::unQuantize(const unsigned short* vecIn) const +{ + btVector3 vecOut; + vecOut.setValue( + (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()), + (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()), + (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ())); + vecOut += m_bvhAabbMin; + return vecOut; +} + +void btOptimizedBvh::swapLeafNodes(int i,int splitIndex) +{ + if (m_useQuantization) + { + btQuantizedBvhNode tmp = m_quantizedLeafNodes[i]; + m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex]; + m_quantizedLeafNodes[splitIndex] = tmp; + } else + { + btOptimizedBvhNode tmp = m_leafNodes[i]; + m_leafNodes[i] = m_leafNodes[splitIndex]; + m_leafNodes[splitIndex] = tmp; + } } +void btOptimizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex) +{ + if (m_useQuantization) + { + m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex]; + } else + { + m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex]; + } +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index cb76cb23340..d5159586344 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -17,103 +17,313 @@ subject to the following restrictions: #define OPTIMIZED_BVH_H -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp -#include - class btStridingMeshInterface; -/// btOptimizedBvhNode contains both internal and leaf node information. -/// It hasn't been optimized yet for storage. Some obvious optimizations are: -/// Removal of the pointers (can already be done, they are not used for traversal) -/// and storing aabbmin/max as quantized integers. -/// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle -/// meshes stored in a non-uniform way (like batches/subparts of triangle-fans -ATTRIBUTE_ALIGNED16 (struct btOptimizedBvhNode) -{ +//Note: currently we have 16 bytes per quantized node +#define MAX_SUBTREE_SIZE_IN_BYTES 2048 - btVector3 m_aabbMin; - btVector3 m_aabbMax; -//these 2 pointers are obsolete, the stackless traversal just uses the escape index - btOptimizedBvhNode* m_leftChild; - btOptimizedBvhNode* m_rightChild; +///btQuantizedBvhNode is a compressed aabb node, 16 bytes. +///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). +ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode +{ + + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrTriangleIndex; + + bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrTriangleIndex >= 0); + } + int getEscapeIndex() const + { + btAssert(!isLeafNode()); + return -m_escapeIndexOrTriangleIndex; + } + int getTriangleIndex() const + { + btAssert(isLeafNode()); + return m_escapeIndexOrTriangleIndex; + } +} +; +/// btOptimizedBvhNode contains both internal and leaf node information. +/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. +ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode +{ + //32 bytes + btVector3 m_aabbMinOrg; + btVector3 m_aabbMaxOrg; + + //4 int m_escapeIndex; + //8 //for child nodes int m_subPart; int m_triangleIndex; + int m_padding[5];//bad, due to alignment + }; + +///btBvhSubtreeInfo provides info to gather a subtree of limited size +ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo +{ +public: + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes, points to the root of the subtree + int m_rootNodeIndex; + //4 bytes + int m_subtreeSize; + int m_padding[3]; + + + void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) + { + m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0]; + m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1]; + m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2]; + m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0]; + m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1]; + m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2]; + } +} +; + + class btNodeOverlapCallback { public: virtual ~btNodeOverlapCallback() {}; - virtual void processNode(const btOptimizedBvhNode* node) = 0; + virtual void processNode(int subPart, int triangleIndex) = 0; }; -#include "LinearMath/btAlignedAllocator.h" -#include "LinearMath/btAlignedObjectArray.h" +#include "../../LinearMath/btAlignedAllocator.h" +#include "../../LinearMath/btAlignedObjectArray.h" -//typedef std::vector< unsigned , allocator_type > container_type; -const unsigned size = (1 << 20); -typedef btAlignedAllocator< btOptimizedBvhNode , size > allocator_type; -//typedef btAlignedObjectArray NodeArray; +///for code readability: typedef btAlignedObjectArray NodeArray; +typedef btAlignedObjectArray QuantizedNodeArray; +typedef btAlignedObjectArray BvhSubtreeInfoArray; ///OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) -class btOptimizedBvh +ATTRIBUTE_ALIGNED16(class) btOptimizedBvh { NodeArray m_leafNodes; + NodeArray m_contiguousNodes; - btOptimizedBvhNode* m_rootNode1; + QuantizedNodeArray m_quantizedLeafNodes; + + QuantizedNodeArray m_quantizedContiguousNodes; - btOptimizedBvhNode* m_contiguousNodes; int m_curNodeIndex; - int m_numNodes; + //quantization data + bool m_useQuantization; + btVector3 m_bvhAabbMin; + btVector3 m_bvhAabbMax; + btVector3 m_bvhQuantization; + enum btTraversalMode + { + TRAVERSAL_STACKLESS = 0, + TRAVERSAL_STACKLESS_CACHE_FRIENDLY, + TRAVERSAL_RECURSIVE + }; -public: - btOptimizedBvh(); + btTraversalMode m_traversalMode; - virtual ~btOptimizedBvh(); - void build(btStridingMeshInterface* triangles); - btOptimizedBvhNode* buildTree (NodeArray& leafNodes,int startIndex,int endIndex); - int calcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex); + BvhSubtreeInfoArray m_SubtreeHeaders; + + + ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) + ///this might be refactored into a virtual, it is usually not calculated at run-time + void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin) + { + if (m_useQuantization) + { + quantizeWithClamp(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; + + } + } + void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) + { + if (m_useQuantization) + { + quantizeWithClamp(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; + } + } + + btVector3 getAabbMin(int nodeIndex) const + { + if (m_useQuantization) + { + return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]); + } + //non-quantized + return m_leafNodes[nodeIndex].m_aabbMinOrg; + + } + btVector3 getAabbMax(int nodeIndex) const + { + if (m_useQuantization) + { + return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]); + } + //non-quantized + return m_leafNodes[nodeIndex].m_aabbMaxOrg; + + } + + void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0)); + + void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) + { + if (m_useQuantization) + { + m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex; + } + else + { + m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex; + } + + } + + void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) + { + if (m_useQuantization) + { + unsigned short int quantizedAabbMin[3]; + unsigned short int quantizedAabbMax[3]; + quantizeWithClamp(quantizedAabbMin,newAabbMin); + quantizeWithClamp(quantizedAabbMax,newAabbMax); + for (int i=0;i<3;i++) + { + if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i]) + m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i]; + + if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i]) + m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i]; + + } + } else + { + //non-quantized + m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin); + m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); + } + } + + void swapLeafNodes(int firstIndex,int secondIndex); + + void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex); + +protected: - int sortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis); - void walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + void buildTree (int startIndex,int endIndex); + + int calcSplittingAxis(int startIndex,int endIndex); + + int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); - void walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; + + ///tree traversal designed for small-memory processors like PS3 SPU + void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + + ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal + void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + + ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal + void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const; - //OptimizedBvhNode* GetRootNode() { return m_rootNode1;} + inline bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const + { + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; + } + + void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); + +public: + btOptimizedBvh(); + + virtual ~btOptimizedBvh(); - int getNumNodes() { return m_numNodes;} + void build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax); void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; void reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void quantizeWithClamp(unsigned short* out, const btVector3& point) const; + + btVector3 unQuantize(const unsigned short* vecIn) const; -}; + ///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees. + void setTraversalMode(btTraversalMode traversalMode) + { + m_traversalMode = traversalMode; + } + + void refit(btStridingMeshInterface* triangles); + + void refitPartial(btStridingMeshInterface* triangles,const btVector3& aabbMin, const btVector3& aabbMax); + + void updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index); + + + QuantizedNodeArray& getQuantizedNodeArray() + { + return m_quantizedContiguousNodes; + } + + BvhSubtreeInfoArray& getSubtreeInfoArray() + { + return m_SubtreeHeaders; + } + +} +; #endif //OPTIMIZED_BVH_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 6f2272cc454..bbc4ba62af6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -16,7 +16,10 @@ subject to the following restrictions: #include btPolyhedralConvexShape::btPolyhedralConvexShape() -:m_optionalHull(0) +:m_localAabbMin(1,1,1), +m_localAabbMax(-1,-1,-1), +m_isLocalAabbValid(false), +m_optionalHull(0) { } @@ -28,16 +31,16 @@ btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const b int i; btVector3 supVec(0,0,0); - btScalar maxDot(-1e30f); + btScalar maxDot(btScalar(-1e30)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < 0.0001f) + if (lenSqr < btScalar(0.0001)) { vec.setValue(1,0,0); } else { - float rlen = 1.f / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); vec *= rlen; } @@ -68,7 +71,7 @@ void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin( for (i=0;i -#include -#include +#include "../../LinearMath/btPoint3.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "btConvexShape.h" ///PolyhedralConvexShape is an interface class for feature based (vertex/edge/face) convex shapes. class btPolyhedralConvexShape : public btConvexShape { +protected: + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; + public: btPolyhedralConvexShape(); @@ -36,6 +41,39 @@ public: virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const + { + + //lazy evaluation of local aabb + btAssert(m_isLocalAabbValid); + + btAssert(m_localAabbMin.getX() <= m_localAabbMax.getX()); + btAssert(m_localAabbMin.getY() <= m_localAabbMax.getY()); + btAssert(m_localAabbMin.getZ() <= m_localAabbMax.getZ()); + + + btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); + + btPoint3 center = trans(localCenter); + + btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + extent += btVector3(margin,margin,margin); + + aabbMin = center - extent; + aabbMax = center + extent; + + + } + + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + void recalcLocalAabb(); virtual int getNumVertices() const = 0 ; virtual int getNumEdges() const = 0; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp index 39e458c0f22..ca65dd03f3e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp @@ -26,14 +26,17 @@ btSphereShape ::btSphereShape (btScalar radius) btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - return btVector3(0.f,0.f,0.f); + (void)vec; + return btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); } void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const { + (void)vectors; + for (int i=0;iinternalProcessTriangleIndex(triangle,part,gfxindex); } @@ -65,11 +67,11 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); } @@ -83,3 +85,40 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde } } +void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax) +{ + + struct AabbCalculationCallback : public btInternalTriangleIndexCallback + { + btVector3 m_aabbMin; + btVector3 m_aabbMax; + + AabbCalculationCallback() + { + m_aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + (void)partId; + (void)triangleIndex; + + m_aabbMin.setMin(triangle[0]); + m_aabbMax.setMax(triangle[0]); + m_aabbMin.setMin(triangle[1]); + m_aabbMax.setMax(triangle[1]); + m_aabbMin.setMin(triangle[2]); + m_aabbMax.setMax(triangle[2]); + } + }; + + //first calculate the total aabb for all triangles + AabbCalculationCallback aabbCallback; + aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax); + + aabbMin = aabbCallback.m_aabbMin; + aabbMax = aabbCallback.m_aabbMax; +} \ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h index 830cbb28200..d7b354b7855 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef STRIDING_MESHINTERFACE_H #define STRIDING_MESHINTERFACE_H -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" #include "btTriangleCallback.h" /// PHY_ScalarType enumerates possible scalar types. @@ -38,7 +38,7 @@ class btStridingMeshInterface btVector3 m_scaling; public: - btStridingMeshInterface() :m_scaling(1.f,1.f,1.f) + btStridingMeshInterface() :m_scaling(btScalar(1.),btScalar(1.),btScalar(1.)) { } @@ -49,6 +49,8 @@ class btStridingMeshInterface void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + ///brute force method to calculate aabb + void calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax); /// get read and write access to a subpart of a triangle mesh /// this subpart has a continuous array of vertices and indices diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp index 7cb40c4fac1..3aa1eda9964 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp @@ -58,6 +58,8 @@ btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const void btBU_Simplex1to4::addVertex(const btPoint3& pt) { m_vertices[m_numVertices++] = pt; + + recalcLocalAabb(); } @@ -176,17 +178,17 @@ int btBU_Simplex1to4::getNumPlanes() const } -void btBU_Simplex1to4::getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const +void btBU_Simplex1to4::getPlane(btVector3&, btPoint3& ,int ) const { } -int btBU_Simplex1to4::getIndex(int i) const +int btBU_Simplex1to4::getIndex(int ) const { return 0; } -bool btBU_Simplex1to4::isInside(const btPoint3& pt,btScalar tolerance) const +bool btBU_Simplex1to4::isInside(const btPoint3& ,btScalar ) const { return false; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h index 9e17a248f84..94bc4ec0fa5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h @@ -17,8 +17,8 @@ subject to the following restrictions: #define BU_SIMPLEX_1TO4_SHAPE -#include -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "btPolyhedralConvexShape.h" +#include "../BroadphaseCollision/btBroadphaseProxy.h" ///BU_Simplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index 9a623403846..3805c519d22 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -17,8 +17,7 @@ subject to the following restrictions: #define BT_TRIANGLE_BUFFER_H #include "btTriangleCallback.h" -//#include -#include "LinearMath/btAlignedObjectArray.h" +#include "../../LinearMath/btAlignedObjectArray.h" struct btTriangle { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h index 7b2337498ec..fbb87bc4fd8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef TRIANGLE_CALLBACK_H #define TRIANGLE_CALLBACK_H -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" class btTriangleCallback diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index 7400d372bee..00847861cf1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -15,15 +15,15 @@ subject to the following restrictions: #include "btTriangleIndexVertexArray.h" -btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride) +btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride) { btIndexedMesh mesh; mesh.m_numTriangles = numTriangles; - mesh.m_triangleIndexBase = triangleIndexBase; + mesh.m_triangleIndexBase = (const unsigned char *)triangleIndexBase; mesh.m_triangleIndexStride = triangleIndexStride; mesh.m_numVertices = numVertices; - mesh.m_vertexBase = vertexBase; + mesh.m_vertexBase = (const unsigned char *)vertexBase; mesh.m_vertexStride = vertexStride; addIndexedMesh(mesh); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index 3ec827c03d9..6ab6a762b39 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -17,40 +17,44 @@ subject to the following restrictions: #define BT_TRIANGLE_INDEX_VERTEX_ARRAY_H #include "btStridingMeshInterface.h" -#include +#include "../../LinearMath/btAlignedObjectArray.h" ///IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements ///instead of the number of indices, we pass the number of triangles ///todo: explain with pictures -struct btIndexedMesh - { - int m_numTriangles; - int* m_triangleIndexBase; - int m_triangleIndexStride; - int m_numVertices; - float* m_vertexBase; - int m_vertexStride; - }; +ATTRIBUTE_ALIGNED16( struct) btIndexedMesh +{ + int m_numTriangles; + const unsigned char * m_triangleIndexBase; + int m_triangleIndexStride; + int m_numVertices; + const unsigned char * m_vertexBase; + int m_vertexStride; + int pad[2]; +} +; + + +typedef btAlignedObjectArray IndexedMeshArray; ///TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. ///Additional meshes can be added using addIndexedMesh ///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays. ///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray. -class btTriangleIndexVertexArray : public btStridingMeshInterface +ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface { - btAlignedObjectArray m_indexedMeshes; + IndexedMeshArray m_indexedMeshes; + int m_pad[3]; public: - - btTriangleIndexVertexArray() { } //just to be backwards compatible - btTriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride); + btTriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride); void addIndexedMesh(const btIndexedMesh& mesh) { @@ -64,19 +68,30 @@ public: /// unLockVertexBase finishes the access to a subpart of the triangle mesh /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished - virtual void unLockVertexBase(int subpart) {} + virtual void unLockVertexBase(int subpart) {(void)subpart;} - virtual void unLockReadOnlyVertexBase(int subpart) const {} + virtual void unLockReadOnlyVertexBase(int subpart) const {(void)subpart;} /// getNumSubParts returns the number of seperate subparts /// each subpart has a continuous array of vertices and indices virtual int getNumSubParts() const { return (int)m_indexedMeshes.size(); } - - virtual void preallocateVertices(int numverts){} - virtual void preallocateIndices(int numindices){} -}; + IndexedMeshArray& getIndexedMeshArray() + { + return m_indexedMeshes; + } + + const IndexedMeshArray& getIndexedMeshArray() const + { + return m_indexedMeshes; + } + + virtual void preallocateVertices(int numverts){(void) numverts;} + virtual void preallocateIndices(int numindices){(void) numindices;} + +} +; #endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp index 489fe1bbcaa..98c54ef45f8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -16,7 +16,6 @@ subject to the following restrictions: #include "btTriangleMesh.h" #include -static int myindices[3] = {0,1,2}; btTriangleMesh::btTriangleMesh () { @@ -25,31 +24,31 @@ btTriangleMesh::btTriangleMesh () void btTriangleMesh::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) { - numverts = 3; - *vertexbase = (unsigned char*)&m_triangles[subpart]; + (void)subpart; + numverts = m_vertices.size(); + *vertexbase = (unsigned char*)&m_vertices[0]; type = PHY_FLOAT; stride = sizeof(btVector3); - - numfaces = 1; - *indexbase = (unsigned char*) &myindices[0]; + numfaces = m_indices.size()/3; + *indexbase = (unsigned char*) &m_indices[0]; indicestype = PHY_INTEGER; - indexstride = sizeof(int); + indexstride = 3*sizeof(int); } void btTriangleMesh::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const { - numverts = 3; - *vertexbase = (unsigned char*)&m_triangles[subpart]; + (void)subpart; + numverts = m_vertices.size(); + *vertexbase = (unsigned char*)&m_vertices[0]; type = PHY_FLOAT; stride = sizeof(btVector3); - - numfaces = 1; - *indexbase = (unsigned char*) &myindices[0]; + numfaces = m_indices.size()/3; + *indexbase = (unsigned char*) &m_indices[0]; indicestype = PHY_INTEGER; - indexstride = sizeof(int); + indexstride = 3*sizeof(int); } @@ -57,5 +56,5 @@ void btTriangleMesh::getLockedReadOnlyVertexIndexBase(const unsigned char **vert int btTriangleMesh::getNumSubParts() const { - return m_triangles.size(); + return 1; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index 1be03d70436..525f5336b48 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -17,42 +17,37 @@ subject to the following restrictions: #ifndef TRIANGLE_MESH_H #define TRIANGLE_MESH_H -#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" -#include -#include "LinearMath/btAlignedObjectArray.h" -struct btMyTriangle -{ - btVector3 m_vert0; - btVector3 m_vert1; - btVector3 m_vert2; -}; +#include "btStridingMeshInterface.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btAlignedObjectArray.h" ///TriangleMesh provides storage for a concave triangle mesh. It can be used as data for the btTriangleMeshShape. class btTriangleMesh : public btStridingMeshInterface { - btAlignedObjectArray m_triangles; + btAlignedObjectArray m_vertices; + btAlignedObjectArray m_indices; public: btTriangleMesh (); void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2) { - btMyTriangle tri; - tri.m_vert0 = vertex0; - tri.m_vert1 = vertex1; - tri.m_vert2 = vertex2; - m_triangles.push_back(tri); + int curIndex = m_indices.size(); + m_vertices.push_back(vertex0); + m_vertices.push_back(vertex1); + m_vertices.push_back(vertex2); + + m_indices.push_back(curIndex++); + m_indices.push_back(curIndex++); + m_indices.push_back(curIndex++); } int getNumTriangles() const { - return m_triangles.size(); + return m_indices.size() / 3; } - const btMyTriangle& getTriangle(int index) const - { - return m_triangles[index]; - } + //StridingMeshInterface interface implementation @@ -62,16 +57,16 @@ class btTriangleMesh : public btStridingMeshInterface /// unLockVertexBase finishes the access to a subpart of the triangle mesh /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished - virtual void unLockVertexBase(int subpart) {} + virtual void unLockVertexBase(int subpart) {(void) subpart;} - virtual void unLockReadOnlyVertexBase(int subpart) const {} + virtual void unLockReadOnlyVertexBase(int subpart) const { (void) subpart;} /// getNumSubParts returns the number of seperate subparts /// each subpart has a continuous array of vertices and indices virtual int getNumSubParts() const; - virtual void preallocateVertices(int numverts){} - virtual void preallocateIndices(int numindices){} + virtual void preallocateVertices(int numverts){(void) numverts;} + virtual void preallocateIndices(int numindices){(void) numindices;} }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp index cd2bf7261d1..ed81897b515 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -40,8 +40,8 @@ btTriangleMeshShape::~btTriangleMeshShape() void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const { - btVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin); - btVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin); + btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); + btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); btMatrix3x3 abs_b = trans.getBasis().absolute(); @@ -62,11 +62,11 @@ void btTriangleMeshShape::recalcLocalAabb() { for (int i=0;i<3;i++) { - btVector3 vec(0.f,0.f,0.f); - vec[i] = 1.f; + btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); m_localAabbMax[i] = tmp[i]+m_collisionMargin; - vec[i] = -1.f; + vec[i] = btScalar(-1.); tmp = localGetSupportingVertex(vec); m_localAabbMin[i] = tmp[i]-m_collisionMargin; } @@ -85,7 +85,7 @@ public: btVector3 m_supportVecLocal; SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans) - : m_supportVertexLocal(0.f,0.f,0.f), m_worldTrans(trans) ,m_maxDot(-1e30f) + : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-1e30)) { m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis(); @@ -93,6 +93,8 @@ public: virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex) { + (void)partId; + (void)triangleIndex; for (int i=0;i<3;i++) { btScalar dot = m_supportVecLocal.dot(triangle[i]); @@ -138,8 +140,7 @@ const btVector3& btTriangleMeshShape::getLocalScaling() const void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { - - struct FilteredCallback : public btInternalTriangleIndexCallback + struct FilteredCallback : public btInternalTriangleIndexCallback { btTriangleCallback* m_callback; btVector3 m_aabbMin; @@ -167,18 +168,19 @@ void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const FilteredCallback filterCallback(callback,aabbMin,aabbMax); m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax); - } + void btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { + (void)mass; //moving concave objects not supported - assert(0); - inertia.setValue(0.f,0.f,0.f); + btAssert(0); + inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } @@ -191,7 +193,7 @@ btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) co SupportVertexCallback supportCallback(vec,ident); - btVector3 aabbMax(1e30f,1e30f,1e30f); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); processAllTriangles(&supportCallback,-aabbMax,aabbMax); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index 4bf000f0c8d..e6173e47640 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -16,17 +16,17 @@ subject to the following restrictions: #ifndef TRIANGLE_MESH_SHAPE_H #define TRIANGLE_MESH_SHAPE_H -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" +#include "btConcaveShape.h" +#include "btStridingMeshInterface.h" ///Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. class btTriangleMeshShape : public btConcaveShape { protected: - btStridingMeshInterface* m_meshInterface; btVector3 m_localAabbMin; btVector3 m_localAabbMax; + btStridingMeshInterface* m_meshInterface; public: @@ -58,6 +58,16 @@ public: virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; + btStridingMeshInterface* getMeshInterface() + { + return m_meshInterface; + } + + const btStridingMeshInterface* getMeshInterface() const + { + return m_meshInterface; + } + //debugging virtual char* getName()const {return "TRIANGLEMESH";} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h index 3284a599ea7..c2e240c051c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define OBB_TRIANGLE_MINKOWSKI_H #include "btConvexShape.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "btBoxShape.h" class btTriangleShape : public btPolyhedralConvexShape { @@ -57,6 +57,7 @@ public: getVertex((i+1)%3,pb); } + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const { // btAssert(0); @@ -110,14 +111,16 @@ public: virtual void getPlaneEquation(int i, btVector3& planeNormal,btPoint3& planeSupport) const { + (void)i; calcNormal(planeNormal); planeSupport = m_vertices1[0]; } virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) { + (void)mass; btAssert(0); - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } virtual bool isInside(const btPoint3& pt,btScalar tolerance) const @@ -166,7 +169,7 @@ public: { calcNormal(penetrationVector); if (index) - penetrationVector *= -1.f; + penetrationVector *= btScalar(-1.); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index ae3ce42e77f..2c565734e97 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -49,28 +49,28 @@ bool btContinuousConvexCollision::calcTimeOfImpact( /// compute linear and angular velocity for this interval, to interpolate btVector3 linVelA,angVelA,linVelB,angVelB; - btTransformUtil::calculateVelocity(fromA,toA,1.f,linVelA,angVelA); - btTransformUtil::calculateVelocity(fromB,toB,1.f,linVelB,angVelB); + btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA); + btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB); btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); btScalar boundingRadiusB = m_convexB->getAngularMotionDisc(); btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; - float radius = 0.001f; + btScalar radius = btScalar(0.001); - btScalar lambda = 0.f; + btScalar lambda = btScalar(0.); btVector3 v(1,0,0); int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(0.f,0.f,0.f); + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); bool hasResult = false; btVector3 c; - float lastLambda = lambda; - //float epsilon = 0.001f; + btScalar lastLambda = lambda; + //btScalar epsilon = btScalar(0.001); int numIter = 0; //first solution, using GJK @@ -79,8 +79,8 @@ bool btContinuousConvexCollision::calcTimeOfImpact( btTransform identityTrans; identityTrans.setIdentity(); - btSphereShape raySphere(0.0f); - raySphere.setMargin(0.f); + btSphereShape raySphere(btScalar(0.0)); + raySphere.setMargin(btScalar(0.)); // result.drawCoordSystem(sphereTr); @@ -116,23 +116,23 @@ bool btContinuousConvexCollision::calcTimeOfImpact( if (numIter > maxIter) return false; //todo: report a failure - float dLambda = 0.f; + btScalar dLambda = btScalar(0.); //calculate safe moving fraction from distance / (linear+rotational velocity) - //float clippedDist = GEN_min(angularConservativeRadius,dist); - //float clippedDist = dist; + //btScalar clippedDist = GEN_min(angularConservativeRadius,dist); + //btScalar clippedDist = dist; - float projectedLinearVelocity = (linVelB-linVelA).dot(n); + btScalar projectedLinearVelocity = (linVelB-linVelA).dot(n); dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); lambda = lambda + dLambda; - if (lambda > 1.f) + if (lambda > btScalar(1.)) return false; - if (lambda < 0.f) + if (lambda < btScalar(0.)) return false; //todo: next check with relative epsilon @@ -159,7 +159,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( gjk.getClosestPoints(input,pointCollector,0); if (pointCollector.m_hasResult) { - if (pointCollector.m_distance < 0.f) + if (pointCollector.m_distance < btScalar(0.)) { //degenerate ?! result.m_fraction = lastLambda; @@ -188,9 +188,9 @@ bool btContinuousConvexCollision::calcTimeOfImpact( //todo: //if movement away from normal, discard result btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin(); - if (result.m_fraction < 1.f) + if (result.m_fraction < btScalar(1.)) { - if (move.dot(result.m_normal) <= 0.f) + if (move.dot(result.m_normal) <= btScalar(0.)) { } } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index 4258d829cca..3101b59993d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -17,11 +17,11 @@ subject to the following restrictions: #ifndef CONVEX_CAST_H #define CONVEX_CAST_H -#include -#include -#include +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btScalar.h" class btMinkowskiSumShape; -#include "LinearMath/btIDebugDraw.h" +#include "../../LinearMath/btIDebugDraw.h" /// btConvexCast is an interface for Casting class btConvexCast @@ -37,11 +37,11 @@ public: { //virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0; - virtual void DebugDraw(btScalar fraction) {} - virtual void drawCoordSystem(const btTransform& trans) {} + virtual void DebugDraw(btScalar fraction) {(void)fraction;} + virtual void drawCoordSystem(const btTransform& trans) {(void)trans;} CastResult() - :m_fraction(1e30f), + :m_fraction(btScalar(1e30)), m_debugDrawer(0) { } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 188c8258c8e..7caeba4be45 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -21,7 +21,7 @@ class btStackAlloc; class btVector3; #include "btSimplexSolverInterface.h" class btConvexShape; -#include "LinearMath/btPoint3.h" +#include "../../LinearMath/btPoint3.h" class btTransform; ///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index e9db97d1b13..15000c1ab61 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H #define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H -#include "LinearMath/btTransform.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" class btStackAlloc; /// This interface is made to be used by an iterative approach to do TimeOfImpact calculations @@ -30,19 +30,18 @@ struct btDiscreteCollisionDetectorInterface struct Result { - void operator delete(void* ptr) {}; virtual ~Result(){} ///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)=0; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)=0; + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0; }; struct ClosestPointInput { ClosestPointInput() - :m_maximumDistanceSquared(1e30f), + :m_maximumDistanceSquared(btScalar(1e30)), m_stackAlloc(0) { } @@ -69,13 +68,13 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result btVector3 m_closestPointInB; btScalar m_distance; //negative means penetration ! - btStorageResult() : m_distance(1e30f) + btStorageResult() : m_distance(btScalar(1e30)) { } virtual ~btStorageResult() {}; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { if (depth < m_distance) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp index bf465b61857..93edffeafd6 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -60,9 +60,9 @@ bool btGjkConvexCast::calcTimeOfImpact( - float radius = 0.01f; + btScalar radius = btScalar(0.01); - btScalar lambda = 0.f; + btScalar lambda = btScalar(0.); btVector3 s = rayFromLocalA.getOrigin(); btVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin(); btVector3 x = s; @@ -71,7 +71,7 @@ bool btGjkConvexCast::calcTimeOfImpact( bool hasResult = false; btVector3 c; - float lastLambda = lambda; + btScalar lastLambda = lambda; //first solution, using GJK @@ -81,8 +81,8 @@ bool btGjkConvexCast::calcTimeOfImpact( btTransform identityTrans; identityTrans.setIdentity(); - btSphereShape raySphere(0.0f); - raySphere.setMargin(0.f); + btSphereShape raySphere(btScalar(0.0)); + raySphere.setMargin(btScalar(0.)); btTransform sphereTr; sphereTr.setIdentity(); @@ -112,7 +112,7 @@ bool btGjkConvexCast::calcTimeOfImpact( if (dist < radius) { //penetration - lastLambda = 1.f; + lastLambda = btScalar(1.); } //not close enough @@ -143,7 +143,7 @@ bool btGjkConvexCast::calcTimeOfImpact( gjk.getClosestPoints(input,pointCollector,0); if (pointCollector.m_hasResult) { - if (pointCollector.m_distance < 0.f) + if (pointCollector.m_distance < btScalar(0.)) { //degeneracy, report a hit result.m_fraction = lastLambda; @@ -160,7 +160,7 @@ bool btGjkConvexCast::calcTimeOfImpact( } - if (lastLambda < 1.f) + if (lastLambda < btScalar(1.)) { result.m_fraction = lastLambda; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h index 66b34b88363..3905c45e6d6 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h @@ -18,9 +18,9 @@ subject to the following restrictions: #ifndef GJK_CONVEX_CAST_H #define GJK_CONVEX_CAST_H -#include +#include "../CollisionShapes/btCollisionMargin.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" #include "btConvexCast.h" class btConvexShape; class btMinkowskiSumShape; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h index 359829f8f31..759b30bb17f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h @@ -21,7 +21,7 @@ Nov.2006 #ifndef _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ #define _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ -#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "../CollisionShapes/btConvexShape.h" class btStackAlloc; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 3d252751c9a..87330493b60 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -26,8 +26,11 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) { + (void)debugDraw; + (void)v; + (void)simplexSolver; - const btScalar radialmargin(0.f); + const btScalar radialmargin(btScalar(0.)); btGjkEpaSolver::sResults results; if(btGjkEpaSolver::Collide( pConvexA,transformA, diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 0ce04dcfad8..f1f3f7f7f6c 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: #endif //must be above the machine epsilon -#define REL_ERROR2 1.0e-6f +#define REL_ERROR2 btScalar(1.0e-6) //temp globals, to improve GJK/EPA/penetration calculations int gNumDeepPenetrationChecks = 0; @@ -36,7 +36,7 @@ int gNumGjkChecks = 0; btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(0.f,0.f,1.f), +:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)), m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), @@ -49,25 +49,25 @@ m_catchDegeneracies(1) void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) { - btScalar distance=0.f; - btVector3 normalInB(0.f,0.f,0.f); + btScalar distance=btScalar(0.); + btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 pointOnA,pointOnB; btTransform localTransA = input.m_transformA; btTransform localTransB = input.m_transformB; - btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * 0.5f; + btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; - float marginA = m_minkowskiA->getMargin(); - float marginB = m_minkowskiB->getMargin(); + btScalar marginA = m_minkowskiA->getMargin(); + btScalar marginB = m_minkowskiB->getMargin(); gNumGjkChecks++; //for CCD we don't use margins if (m_ignoreMargin) { - marginA = 0.f; - marginB = 0.f; + marginA = btScalar(0.); + marginB = btScalar(0.); } m_curIter = 0; @@ -83,7 +83,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { btScalar squaredDistance = SIMD_INFINITY; - btScalar delta = 0.f; + btScalar delta = btScalar(0.); btScalar margin = marginA + marginB; @@ -91,7 +91,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& m_simplexSolver->reset(); - while (true) + for ( ; ; ) + //while (true) { btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); @@ -120,12 +121,12 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& break; } // are we getting any closer ? - float f0 = squaredDistance - delta; - float f1 = squaredDistance * REL_ERROR2; + btScalar f0 = squaredDistance - delta; + btScalar f1 = squaredDistance * REL_ERROR2; if (f0 <= f1) { - if (f0 <= 0.f) + if (f0 <= btScalar(0.)) { m_degenerateSimplex = 2; } @@ -191,7 +192,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { m_simplexSolver->compute_points(pointOnA, pointOnB); normalInB = pointOnA-pointOnB; - float lenSqr = m_cachedSeparatingAxis.length2(); + btScalar lenSqr = m_cachedSeparatingAxis.length2(); //valid normal if (lenSqr < 0.0001) { @@ -199,14 +200,14 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& } if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) { - float rlen = 1.f / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); normalInB *= rlen; //normalize btScalar s = btSqrt(squaredDistance); btAssert(s > btScalar(0.0)); pointOnA -= m_cachedSeparatingAxis * (marginA / s); pointOnB += m_cachedSeparatingAxis * (marginB / s); - distance = ((1.f/rlen) - margin); + distance = ((btScalar(1.)/rlen) - margin); isValid = true; m_lastUsedMethod = 1; @@ -243,11 +244,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (isValid2) { btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - float lenSqr = tmpNormalInB.length2(); + btScalar lenSqr = tmpNormalInB.length2(); if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { tmpNormalInB /= btSqrt(lenSqr); - float distance2 = -(tmpPointOnA-tmpPointOnB).length(); + btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); //only replace valid penetrations when the result is deeper (check) if (!isValid || (distance2 < distance)) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index 09c1669bd78..af0fe32f6c7 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -20,9 +20,8 @@ subject to the following restrictions: #define GJK_PAIR_DETECTOR_H #include "btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btPoint3.h" - -#include +#include "../../LinearMath/btPoint3.h" +#include "../CollisionShapes/btCollisionMargin.h" class btConvexShape; #include "btSimplexSolverInterface.h" diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 00a9206fef0..f6a893151da 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef MANIFOLD_CONTACT_POINT_H #define MANIFOLD_CONTACT_POINT_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransformUtil.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransformUtil.h" @@ -29,7 +29,8 @@ class btManifoldPoint { public: btManifoldPoint() - :m_userPersistentData(0) + :m_userPersistentData(0), + m_lifeTime(0) { } @@ -40,8 +41,8 @@ class btManifoldPoint m_localPointB( pointB ), m_normalWorldOnB( normal ), m_distance1( distance ), - m_combinedFriction(0.f), - m_combinedRestitution(0.f), + m_combinedFriction(btScalar(0.)), + m_combinedRestitution(btScalar(0.)), m_userPersistentData(0), m_lifeTime(0) { @@ -58,16 +59,16 @@ class btManifoldPoint btVector3 m_positionWorldOnA; btVector3 m_normalWorldOnB; - float m_distance1; - float m_combinedFriction; - float m_combinedRestitution; + btScalar m_distance1; + btScalar m_combinedFriction; + btScalar m_combinedRestitution; - void* m_userPersistentData; + mutable void* m_userPersistentData; int m_lifeTime;//lifetime of the contactpoint in frames - float getDistance() const + btScalar getDistance() const { return m_distance1; } @@ -76,17 +77,17 @@ class btManifoldPoint return m_lifeTime; } - btVector3 getPositionWorldOnA() { + const btVector3& getPositionWorldOnA() const { return m_positionWorldOnA; // return m_positionWorldOnB + m_normalWorldOnB * m_distance1; } - const btVector3& getPositionWorldOnB() + const btVector3& getPositionWorldOnB() const { return m_positionWorldOnB; } - void setDistance(float dist) + void setDistance(btScalar dist) { m_distance1 = dist; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index de6749a9b72..c4bab3a134a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -25,48 +25,48 @@ subject to the following restrictions: #define NUM_UNITSPHERE_POINTS 42 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), -btVector3(-0.276388f , -0.850649f,-0.447219f), -btVector3(-0.894426f , -0.000000f,-0.447216f), -btVector3(-0.276388f , 0.850649f,-0.447220f), -btVector3(0.723608f , 0.525725f,-0.447219f), -btVector3(0.276388f , -0.850649f,0.447220f), -btVector3(-0.723608f , -0.525725f,0.447219f), -btVector3(-0.723608f , 0.525725f,0.447219f), -btVector3(0.276388f , 0.850649f,0.447219f), -btVector3(0.894426f , 0.000000f,0.447216f), -btVector3(-0.000000f , 0.000000f,1.000000f), -btVector3(0.425323f , -0.309011f,-0.850654f), -btVector3(-0.162456f , -0.499995f,-0.850654f), -btVector3(0.262869f , -0.809012f,-0.525738f), -btVector3(0.425323f , 0.309011f,-0.850654f), -btVector3(0.850648f , -0.000000f,-0.525736f), -btVector3(-0.525730f , -0.000000f,-0.850652f), -btVector3(-0.688190f , -0.499997f,-0.525736f), -btVector3(-0.162456f , 0.499995f,-0.850654f), -btVector3(-0.688190f , 0.499997f,-0.525736f), -btVector3(0.262869f , 0.809012f,-0.525738f), -btVector3(0.951058f , 0.309013f,0.000000f), -btVector3(0.951058f , -0.309013f,0.000000f), -btVector3(0.587786f , -0.809017f,0.000000f), -btVector3(0.000000f , -1.000000f,0.000000f), -btVector3(-0.587786f , -0.809017f,0.000000f), -btVector3(-0.951058f , -0.309013f,-0.000000f), -btVector3(-0.951058f , 0.309013f,-0.000000f), -btVector3(-0.587786f , 0.809017f,-0.000000f), -btVector3(-0.000000f , 1.000000f,-0.000000f), -btVector3(0.587786f , 0.809017f,-0.000000f), -btVector3(0.688190f , -0.499997f,0.525736f), -btVector3(-0.262869f , -0.809012f,0.525738f), -btVector3(-0.850648f , 0.000000f,0.525736f), -btVector3(-0.262869f , 0.809012f,0.525738f), -btVector3(0.688190f , 0.499997f,0.525736f), -btVector3(0.525730f , 0.000000f,0.850652f), -btVector3(0.162456f , -0.499995f,0.850654f), -btVector3(-0.425323f , -0.309011f,0.850654f), -btVector3(-0.425323f , 0.309011f,0.850654f), -btVector3(0.162456f , 0.499995f,0.850654f) +btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), +btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), +btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), +btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), +btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), +btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), +btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), +btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), +btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), +btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), +btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), +btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), +btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), +btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), +btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), +btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), +btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), +btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), +btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), +btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), +btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), +btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), +btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), +btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), +btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), +btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), +btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), +btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), +btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), +btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), +btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), +btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), +btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), +btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), +btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), +btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), +btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), +btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), +btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), +btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), +btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), +btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) }; @@ -78,6 +78,9 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s ) { + (void)stackAlloc; + (void)v; + struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result { @@ -88,13 +91,17 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 m_normalOnBInWorld; btVector3 m_pointInWorld; - float m_depth; + btScalar m_depth; bool m_hasResult; virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) { + (void)partId0; + (void)index0; + (void)partId1; + (void)index1; } - void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { m_normalOnBInWorld = normalOnBInWorld; m_pointInWorld = pointInWorld; @@ -104,7 +111,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s }; //just take fixed number of orientation, and sample the penetration depth in that direction - float minProj = 1e30f; + btScalar minProj = btScalar(1e30); btVector3 minNorm; btVector3 minVertex; btVector3 minA,minB; @@ -180,7 +187,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s pWorld = transA(pInA); qWorld = transB(qInB); w = qWorld - pWorld; - float delta = norm.dot(w); + btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) { @@ -234,7 +241,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s pWorld = transA(pInA); qWorld = transB(qInB); w = qWorld - pWorld; - float delta = norm.dot(w); + btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) { @@ -251,7 +258,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minA += minNorm*convexA->getMargin(); minB -= minNorm*convexB->getMargin(); //no penetration - if (minProj < 0.f) + if (minProj < btScalar(0.)) return false; minProj += (convexA->getMargin() + convexB->getMargin()); @@ -268,7 +275,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s debugDraw->drawLine(minA,minB,color); color = btVector3 (1,1,1); btVector3 vec = minB-minA; - float prj2 = minNorm.dot(vec); + btScalar prj2 = minNorm.dot(vec); debugDraw->drawLine(minA,minA+(minNorm*minProj),color); } @@ -292,16 +299,16 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_transformA = displacedTrans; input.m_transformB = transB; - input.m_maximumDistanceSquared = 1e30f;//minProj; + input.m_maximumDistanceSquared = btScalar(1e30);//minProj; btIntermediateResult res; gjkdet.getClosestPoints(input,res,debugDraw); - float correctedMinNorm = minProj - res.m_depth; + btScalar correctedMinNorm = minProj - res.m_depth; //the penetration depth is over-estimated, relax it - float penetration_relaxation= 1.f; + btScalar penetration_relaxation= btScalar(1.); minNorm*=penetration_relaxation; if (res.m_hasResult) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index ee2be163063..08cb3ed334d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -18,7 +18,7 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include -float gContactBreakingThreshold = 0.02f; +btScalar gContactBreakingThreshold = btScalar(0.02); ContactDestroyedCallback gContactDestroyedCallback = 0; @@ -100,7 +100,7 @@ int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) int maxPenetrationIndex = -1; #define KEEP_DEEPEST_POINT 1 #ifdef KEEP_DEEPEST_POINT - float maxPenetration = pt.getDistance(); + btScalar maxPenetration = pt.getDistance(); for (int i=0;i<4;i++) { if (m_pointCache[i].getDistance() < maxPenetration) @@ -111,7 +111,7 @@ int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) } #endif //KEEP_DEEPEST_POINT - btScalar res0(0.f),res1(0.f),res2(0.f),res3(0.f); + btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.)); if (maxPenetrationIndex != 0) { btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; @@ -193,7 +193,7 @@ void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint) replaceContactPoint(newPoint,insertIndex); } -float btPersistentManifold::getContactBreakingThreshold() const +btScalar btPersistentManifold::getContactBreakingThreshold() const { return gContactBreakingThreshold; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index c2372b648f6..a5918b84db3 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -17,14 +17,14 @@ subject to the following restrictions: #define PERSISTENT_MANIFOLD_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" #include "btManifoldPoint.h" struct btCollisionResult; ///contact breaking and merging threshold -extern float gContactBreakingThreshold; +extern btScalar gContactBreakingThreshold; typedef bool (*ContactDestroyedCallback)(void* userPersistentData); extern ContactDestroyedCallback gContactDestroyedCallback; @@ -36,7 +36,7 @@ extern ContactDestroyedCallback gContactDestroyedCallback; ///btPersistentManifold maintains contact points, and reduces them to 4. ///It does contact filtering/contact reduction. -class btPersistentManifold +ATTRIBUTE_ALIGNED16( class) btPersistentManifold { btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; @@ -97,7 +97,7 @@ public: } /// todo: get this margin from the current physics / collision environment - float getContactBreakingThreshold() const; + btScalar getContactBreakingThreshold() const; int getCacheEntry(const btManifoldPoint& newPoint) const; @@ -108,18 +108,36 @@ public: clearUserCache(m_pointCache[index]); int lastUsedIndex = getNumContacts() - 1; - m_pointCache[index] = m_pointCache[lastUsedIndex]; - //get rid of duplicated userPersistentData pointer - m_pointCache[lastUsedIndex].m_userPersistentData = 0; +// m_pointCache[index] = m_pointCache[lastUsedIndex]; + if(index != lastUsedIndex) + { + m_pointCache[index] = m_pointCache[lastUsedIndex]; + //get rid of duplicated userPersistentData pointer + m_pointCache[lastUsedIndex].m_userPersistentData = 0; + } + + btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0); m_cachedPoints--; } void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex) { - assert(validContactDistance(newPoint)); + btAssert(validContactDistance(newPoint)); - clearUserCache(m_pointCache[insertIndex]); +#define MAINTAIN_PERSISTENCY 1 +#ifdef MAINTAIN_PERSISTENCY + int lifeTime = m_pointCache[insertIndex].getLifeTime(); + btAssert(lifeTime>=0); + void* cache = m_pointCache[insertIndex].m_userPersistentData; m_pointCache[insertIndex] = newPoint; + + m_pointCache[insertIndex].m_userPersistentData = cache; + m_pointCache[insertIndex].m_lifeTime = lifeTime; +#else + clearUserCache(m_pointCache[insertIndex]); + m_pointCache[insertIndex] = newPoint; + +#endif } bool validContactDistance(const btManifoldPoint& pt) const @@ -133,7 +151,10 @@ public: -}; +} +; + + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h index a51e2d5e13b..6262f44b9f1 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h @@ -31,16 +31,20 @@ struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result bool m_hasResult; btPointCollector () - : m_distance(1e30f),m_hasResult(false) + : m_distance(btScalar(1e30)),m_hasResult(false) { } virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) { + (void)partId0; + (void)index0; + (void)partId1; + (void)index1; //?? } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { if (depth< m_distance) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index 88ee005792c..31b91467777 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -20,7 +20,7 @@ btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const : m_from(from), m_to(to), - m_hitFraction(1.f) + m_hitFraction(btScalar(1.)) { } @@ -40,19 +40,19 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, btVector3 triangleNormal; triangleNormal = v10.cross( v20 ); - const float dist = vert0.dot(triangleNormal); - float dist_a = triangleNormal.dot(m_from) ; + const btScalar dist = vert0.dot(triangleNormal); + btScalar dist_a = triangleNormal.dot(m_from) ; dist_a-= dist; - float dist_b = triangleNormal.dot(m_to); + btScalar dist_b = triangleNormal.dot(m_to); dist_b -= dist; - if ( dist_a * dist_b >= 0.0f) + if ( dist_a * dist_b >= btScalar(0.0) ) { return ; // same sign } - const float proj_length=dist_a-dist_b; - const float distance = (dist_a)/(proj_length); + const btScalar proj_length=dist_a-dist_b; + const btScalar distance = (dist_a)/(proj_length); // Now we have the intersection point on the plane, we'll see if it's inside the triangle // Add an epsilon as a tolerance for the raycast, // in case the ray hits exacly on the edge of the triangle. @@ -62,27 +62,27 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, { - float edge_tolerance =triangleNormal.length2(); - edge_tolerance *= -0.0001f; + btScalar edge_tolerance =triangleNormal.length2(); + edge_tolerance *= btScalar(-0.0001); btVector3 point; point.setInterpolate3( m_from, m_to, distance); { btVector3 v0p; v0p = vert0 - point; btVector3 v1p; v1p = vert1 - point; btVector3 cp0; cp0 = v0p.cross( v1p ); - if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance) + if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) { btVector3 v2p; v2p = vert2 - point; btVector3 cp1; cp1 = v1p.cross( v2p); - if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance) + if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) { btVector3 cp2; cp2 = v2p.cross(v0p); - if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance) + if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) { if ( dist_a > 0 ) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index fbb51d30522..a0bbc9f8fe9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef RAYCAST_TRI_CALLBACK_H #define RAYCAST_TRI_CALLBACK_H -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "../CollisionShapes/btTriangleCallback.h" struct btBroadphaseProxy; @@ -28,13 +28,13 @@ public: btVector3 m_from; btVector3 m_to; - float m_hitFraction; + btScalar m_hitFraction; btTriangleRaycastCallback(const btVector3& from,const btVector3& to); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - virtual float reportHit(const btVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0; + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0; }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h index cf65f46505b..58393b2eab9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -18,8 +18,8 @@ subject to the following restrictions: #ifndef SIMPLEX_SOLVER_INTERFACE_H #define SIMPLEX_SOLVER_INTERFACE_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btPoint3.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btPoint3.h" #define NO_VIRTUAL_INTERFACE 1 #ifdef NO_VIRTUAL_INTERFACE diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index dc995ca1f72..1ab336b4d54 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -28,8 +28,11 @@ m_convexA(convexA),m_convexB(convexB) ///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. ///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 +#ifdef BT_USE_DOUBLE_PRECISION +#define MAX_ITERATIONS 64 +#else #define MAX_ITERATIONS 32 - +#endif bool btSubsimplexConvexCast::calcTimeOfImpact( const btTransform& fromA, const btTransform& toA, @@ -52,9 +55,9 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( convex->setTransformB(btTransform(rayFromLocalA.getBasis())); - //float radius = 0.01f; + //btScalar radius = btScalar(0.01); - btScalar lambda = 0.f; + btScalar lambda = btScalar(0.); //todo: need to verify this: //because of minkowski difference, we need the inverse direction @@ -69,27 +72,30 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(0.f,0.f,0.f); + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); bool hasResult = false; btVector3 c; - float lastLambda = lambda; - + btScalar lastLambda = lambda; - float dist2 = v.length2(); - float epsilon = 0.0001f; + btScalar dist2 = v.length2(); +#ifdef BT_USE_DOUBLE_PRECISION + btScalar epsilon = btScalar(0.0001); +#else + btScalar epsilon = btScalar(0.0001); +#endif //BT_USE_DOUBLE_PRECISION btVector3 w,p; - float VdotR; + btScalar VdotR; while ( (dist2 > epsilon) && maxIter--) { p = convex->localGetSupportingVertex( v); w = x - p; - float VdotW = v.dot(w); + btScalar VdotW = v.dot(w); - if ( VdotW > 0.f) + if ( VdotW > btScalar(0.)) { VdotR = v.dot(r); @@ -117,7 +123,7 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( //printf("numverts = %i\n",m_simplexSolver->numVertices()); } else { - dist2 = 0.f; + dist2 = btScalar(0.); } } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index 23d66a3bbc8..105b7eccefa 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -70,7 +70,7 @@ void btVoronoiSimplexSolver::reset() m_cachedValidClosest = false; m_numVertices = 0; m_needsUpdate = true; - m_lastW = btVector3(1e30f,1e30f,1e30f); + m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); m_cachedBC.reset(); } @@ -109,7 +109,7 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() m_cachedP2 = m_simplexPointsQ[0]; m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] m_cachedBC.reset(); - m_cachedBC.setBarycentricCoordinates(1.f,0.f,0.f,0.f); + m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); m_cachedValidClosest = m_cachedBC.isValid(); break; }; @@ -120,13 +120,13 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() const btVector3& to = m_simplexVectorW[1]; btVector3 nearest; - btVector3 p (0.f,0.f,0.f); + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); 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; @@ -156,38 +156,36 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() m_cachedValidClosest = m_cachedBC.isValid(); break; } - case 3: - { - //closest point origin from triangle - btVector3 p (0.f,0.f,0.f); - - const btVector3& a = m_simplexVectorW[0]; - const btVector3& b = m_simplexVectorW[1]; - const btVector3& c = m_simplexVectorW[2]; + case 3: + { + //closest point origin from triangle + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - closestPtPointTriangle(p,a,b,c,m_cachedBC); - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + const btVector3& a = m_simplexVectorW[0]; + const btVector3& b = m_simplexVectorW[1]; + const btVector3& c = m_simplexVectorW[2]; - m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + closestPtPointTriangle(p,a,b,c,m_cachedBC); + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; - m_cachedV = m_cachedP1-m_cachedP2; + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; - reduceVertices (m_cachedBC.m_usedVertices); - m_cachedValidClosest = m_cachedBC.isValid(); + m_cachedV = m_cachedP1-m_cachedP2; - break; + reduceVertices (m_cachedBC.m_usedVertices); + m_cachedValidClosest = m_cachedBC.isValid(); + + break; } case 4: { - btVector3 p (0.f,0.f,0.f); + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); const btVector3& a = m_simplexVectorW[0]; const btVector3& b = m_simplexVectorW[1]; @@ -222,7 +220,7 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() { m_cachedValidClosest = true; //degenerate case == false, penetration = true + zero - m_cachedV.setValue(0.f,0.f,0.f); + m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } break; } @@ -256,7 +254,7 @@ bool btVoronoiSimplexSolver::closest(btVector3& v) btScalar btVoronoiSimplexSolver::maxVertex() { int i, numverts = numVertices(); - btScalar maxV = 0.f; + btScalar maxV = btScalar(0.); for (i=0;i= 0.0f && d4 <= d3) + btScalar d3 = ab.dot(bp); + btScalar d4 = ac.dot(bp); + if (d3 >= btScalar(0.0) && d4 <= d3) { result.m_closestPointOnSimplex = b; result.m_usedVertices.usedVertexB = true; @@ -358,9 +356,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP return true; // b; // barycentric coordinates (0,1,0) } // Check if P in edge region of AB, if so return projection of P onto AB - float vc = d1*d4 - d3*d2; - if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) { - float v = d1 / (d1 - d3); + btScalar vc = d1*d4 - d3*d2; + if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { + btScalar v = d1 / (d1 - d3); result.m_closestPointOnSimplex = a + v * ab; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; @@ -371,9 +369,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP // Check if P in vertex region outside C btVector3 cp = p - c; - float d5 = ab.dot(cp); - float d6 = ac.dot(cp); - if (d6 >= 0.0f && d5 <= d6) + btScalar d5 = ab.dot(cp); + btScalar d6 = ac.dot(cp); + if (d6 >= btScalar(0.0) && d5 <= d6) { result.m_closestPointOnSimplex = c; result.m_usedVertices.usedVertexC = true; @@ -382,9 +380,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP } // Check if P in edge region of AC, if so return projection of P onto AC - float vb = d5*d2 - d1*d6; - if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) { - float w = d2 / (d2 - d6); + btScalar vb = d5*d2 - d1*d6; + if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { + btScalar w = d2 / (d2 - d6); result.m_closestPointOnSimplex = a + w * ac; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexC = true; @@ -394,9 +392,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP } // Check if P in edge region of BC, if so return projection of P onto BC - float va = d3*d6 - d5*d4; - if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) { - float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + btScalar va = d3*d6 - d5*d4; + if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { + btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); result.m_closestPointOnSimplex = b + w * (c - b); result.m_usedVertices.usedVertexB = true; @@ -407,9 +405,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - float denom = 1.0f / (va + vb + vc); - float v = vb * denom; - float w = vc * denom; + btScalar denom = btScalar(1.0) / (va + vb + vc); + btScalar v = vb * denom; + btScalar w = vc * denom; result.m_closestPointOnSimplex = a + ab * v + ac * w; result.m_usedVertices.usedVertexA = true; @@ -418,7 +416,7 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP result.setBarycentricCoordinates(1-v-w,v,w); return true; -// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w +// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w } @@ -431,18 +429,26 @@ int btVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint { btVector3 normal = (b-a).cross(c-a); - float signp = (p - a).dot(normal); // [AP AB AC] - float signd = (d - a).dot( normal); // [AD AB AC] + btScalar signp = (p - a).dot(normal); // [AP AB AC] + btScalar signd = (d - a).dot( normal); // [AD AB AC] #ifdef CATCH_DEGENERATE_TETRAHEDRON - if (signd * signd < (1e-4f * 1e-4f)) +#ifdef BT_USE_DOUBLE_PRECISION +if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) + { + return -1; + } +#else + if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) { // printf("affine dependent/degenerate\n");// return -1; } +#endif + #endif // Points on opposite sides if expression signs are opposite - return signp * signd < 0.f; + return signp * signd < btScalar(0.); } @@ -475,14 +481,14 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const } - float bestSqDist = FLT_MAX; + btScalar bestSqDist = FLT_MAX; // If point outside face abc then compute closest point on abc if (pointOutsideABC) { closestPtPointTriangle(p, a, b, c,tempResult); btPoint3 q = tempResult.m_closestPointOnSimplex; - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; @@ -510,13 +516,14 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; finalResult.setBarycentricCoordinates( @@ -537,15 +544,16 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; + + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( tempResult.m_barycentricCoords[VERTA], tempResult.m_barycentricCoords[VERTC], @@ -563,15 +571,16 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const closestPtPointTriangle(p, b, d, c,tempResult); btPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); + // finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( 0, diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index 8b743996915..356d335bc93 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -55,7 +55,7 @@ struct btSubSimplexClosestResult //stores the simplex vertex-usage, using the MASK, // if m_usedVertices & MASK then the related vertex is used btUsageBitfield m_usedVertices; - float m_barycentricCoords[4]; + btScalar m_barycentricCoords[4]; bool m_degenerate; void reset() @@ -66,15 +66,15 @@ struct btSubSimplexClosestResult } bool isValid() { - bool valid = (m_barycentricCoords[0] >= 0.f) && - (m_barycentricCoords[1] >= 0.f) && - (m_barycentricCoords[2] >= 0.f) && - (m_barycentricCoords[3] >= 0.f); + bool valid = (m_barycentricCoords[0] >= btScalar(0.)) && + (m_barycentricCoords[1] >= btScalar(0.)) && + (m_barycentricCoords[2] >= btScalar(0.)) && + (m_barycentricCoords[3] >= btScalar(0.)); return valid; } - void setBarycentricCoordinates(float a=0.f,float b=0.f,float c=0.f,float d=0.f) + void setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.)) { m_barycentricCoords[0] = a; m_barycentricCoords[1] = b; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index ce90f798c04..7e8458c2c7b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -16,12 +16,16 @@ subject to the following restrictions: #ifndef CONSTRAINT_SOLVER_H #define CONSTRAINT_SOLVER_H +#include "../../LinearMath/btScalar.h" + class btPersistentManifold; class btRigidBody; +class btCollisionObject; class btTypedConstraint; struct btContactSolverInfo; struct btBroadphaseProxy; class btIDebugDraw; +class btStackAlloc; /// btConstraintSolver provides solver interface class btConstraintSolver @@ -31,7 +35,7 @@ public: virtual ~btConstraintSolver() {} - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0; + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 1b85a0eea42..bb3fe832592 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -30,13 +30,17 @@ subject to the following restrictions: //bilateral constraint between two dynamic objects void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep) + btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep) { - float normalLenSqr = normal.length2(); - ASSERT2(fabs(normalLenSqr) < 1.1f); - if (normalLenSqr > 1.1f) + (void)timeStep; + (void)distance; + + + btScalar normalLenSqr = normal.length2(); + ASSERT2(btFabs(normalLenSqr) < btScalar(1.1)); + if (normalLenSqr > btScalar(1.1)) { - impulse = 0.f; + impulse = btScalar(0.); return; } btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); @@ -54,24 +58,24 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, body2.getInvInertiaDiagLocal(),body2.getInvMass()); btScalar jacDiagAB = jac.getDiagonal(); - btScalar jacDiagABInv = 1.f / jacDiagAB; + btScalar jacDiagABInv = btScalar(1.) / jacDiagAB; btScalar rel_vel = jac.getRelativeVelocity( body1.getLinearVelocity(), body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), body2.getLinearVelocity(), body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); - float a; + btScalar a; a=jacDiagABInv; rel_vel = normal.dot(vel); //todo: move this into proper structure - btScalar contactDamping = 0.2f; + btScalar contactDamping = btScalar(0.2); #ifdef ONLY_USE_LINEAR_MASS - btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); + btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass()); impulse = - contactDamping * rel_vel * massTerm; #else btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; @@ -82,19 +86,20 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, //response between two dynamic objects with friction -float resolveSingleCollision( +btScalar resolveSingleCollision( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { - const btVector3& pos1 = contactPoint.getPositionWorldOnA(); - const btVector3& pos2 = contactPoint.getPositionWorldOnB(); + const btVector3& pos1_ = contactPoint.getPositionWorldOnA(); + const btVector3& pos2_ = contactPoint.getPositionWorldOnB(); const btVector3& normal = contactPoint.m_normalWorldOnB; - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + //constant over all iterations + btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition(); btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); @@ -102,11 +107,11 @@ float resolveSingleCollision( btScalar rel_vel; rel_vel = normal.dot(vel); - btScalar Kfps = 1.f / solverInfo.m_timeStep ; + btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ; - // float damping = solverInfo.m_damping ; - float Kerp = solverInfo.m_erp; - float Kcor = Kerp *Kfps; + // btScalar damping = solverInfo.m_damping ; + btScalar Kerp = solverInfo.m_erp; + btScalar Kcor = Kerp *Kfps; btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); @@ -121,9 +126,9 @@ float resolveSingleCollision( btScalar normalImpulse = penetrationImpulse+velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - float oldNormalImpulse = cpd->m_appliedImpulse; - float sum = oldNormalImpulse + normalImpulse; - cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum; + btScalar oldNormalImpulse = cpd->m_appliedImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; @@ -145,13 +150,15 @@ float resolveSingleCollision( } -float resolveSingleFriction( +btScalar resolveSingleFriction( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { + (void)solverInfo; + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); const btVector3& pos2 = contactPoint.getPositionWorldOnB(); @@ -161,11 +168,11 @@ float resolveSingleFriction( btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); - float combinedFriction = cpd->m_friction; + btScalar combinedFriction = cpd->m_friction; btScalar limit = cpd->m_appliedImpulse * combinedFriction; - if (cpd->m_appliedImpulse>0.f) + if (cpd->m_appliedImpulse>btScalar(0.)) //friction { //apply friction in the 2 tangential directions @@ -183,7 +190,7 @@ float resolveSingleFriction( // calculate j that moves us to zero relative velocity j1 = -vrel * cpd->m_jacDiagABInvTangent0; - float oldTangentImpulse = cpd->m_accumulatedTangentImpulse0; + btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0; cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1; GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit); GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit); @@ -197,7 +204,7 @@ float resolveSingleFriction( // calculate j that moves us to zero relative velocity j2 = -vrel * cpd->m_jacDiagABInvTangent1; - float oldTangentImpulse = cpd->m_accumulatedTangentImpulse1; + btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1; cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2; GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit); GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit); @@ -226,13 +233,15 @@ float resolveSingleFriction( } -float resolveSingleFrictionOriginal( +btScalar resolveSingleFrictionOriginal( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { + (void)solverInfo; + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); const btVector3& pos2 = contactPoint.getPositionWorldOnB(); @@ -242,10 +251,10 @@ float resolveSingleFrictionOriginal( btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); - float combinedFriction = cpd->m_friction; + btScalar combinedFriction = cpd->m_friction; btScalar limit = cpd->m_appliedImpulse * combinedFriction; - //if (contactPoint.m_appliedImpulse>0.f) + //if (contactPoint.m_appliedImpulse>btScalar(0.)) //friction { //apply friction in the 2 tangential directions @@ -260,7 +269,7 @@ float resolveSingleFrictionOriginal( // calculate j that moves us to zero relative velocity btScalar j = -vrel * cpd->m_jacDiagABInvTangent0; - float total = cpd->m_accumulatedTangentImpulse0 + j; + btScalar total = cpd->m_accumulatedTangentImpulse0 + j; GEN_set_min(total, limit); GEN_set_max(total, -limit); j = total - cpd->m_accumulatedTangentImpulse0; @@ -280,7 +289,7 @@ float resolveSingleFrictionOriginal( // calculate j that moves us to zero relative velocity btScalar j = -vrel * cpd->m_jacDiagABInvTangent1; - float total = cpd->m_accumulatedTangentImpulse1 + j; + btScalar total = cpd->m_accumulatedTangentImpulse1 + j; GEN_set_min(total, limit); GEN_set_max(total, -limit); j = total - cpd->m_accumulatedTangentImpulse1; @@ -295,7 +304,7 @@ float resolveSingleFrictionOriginal( //velocity + friction //response between two dynamic objects with friction -float resolveSingleCollisionCombined( +btScalar resolveSingleCollisionCombined( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, @@ -315,11 +324,11 @@ float resolveSingleCollisionCombined( btScalar rel_vel; rel_vel = normal.dot(vel); - btScalar Kfps = 1.f / solverInfo.m_timeStep ; + btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ; - //float damping = solverInfo.m_damping ; - float Kerp = solverInfo.m_erp; - float Kcor = Kerp *Kfps; + //btScalar damping = solverInfo.m_damping ; + btScalar Kerp = solverInfo.m_erp; + btScalar Kcor = Kerp *Kfps; btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); @@ -334,9 +343,9 @@ float resolveSingleCollisionCombined( btScalar normalImpulse = penetrationImpulse+velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - float oldNormalImpulse = cpd->m_appliedImpulse; - float sum = oldNormalImpulse + normalImpulse; - cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum; + btScalar oldNormalImpulse = cpd->m_appliedImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; @@ -367,7 +376,7 @@ float resolveSingleCollisionCombined( btVector3 lat_vel = vel - normal * rel_vel; btScalar lat_rel_vel = lat_vel.length(); - float combinedFriction = cpd->m_friction; + btScalar combinedFriction = cpd->m_friction; if (cpd->m_appliedImpulse > 0) if (lat_rel_vel > SIMD_EPSILON) @@ -390,12 +399,19 @@ float resolveSingleCollisionCombined( return normalImpulse; } -float resolveSingleFrictionEmpty( + +btScalar resolveSingleFrictionEmpty( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { - return 0.f; + (void)contactPoint; + (void)body1; + (void)body2; + (void)solverInfo; + + + return btScalar(0.); }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index d88ba0d8ed4..0834deddeac 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -19,8 +19,8 @@ subject to the following restrictions: //todo: make into a proper class working with the iterative constraint solver class btRigidBody; -#include "LinearMath/btVector3.h" -#include "LinearMath/btScalar.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btScalar.h" struct btContactSolverInfo; class btManifoldPoint; @@ -33,7 +33,7 @@ enum { }; -typedef float (*ContactSolverFunc)(btRigidBody& body1, +typedef btScalar (*ContactSolverFunc)(btRigidBody& body1, btRigidBody& body2, class btManifoldPoint& contactPoint, const btContactSolverInfo& info); @@ -42,15 +42,15 @@ typedef float (*ContactSolverFunc)(btRigidBody& body1, struct btConstraintPersistentData { inline btConstraintPersistentData() - :m_appliedImpulse(0.f), - m_prevAppliedImpulse(0.f), - m_accumulatedTangentImpulse0(0.f), - m_accumulatedTangentImpulse1(0.f), - m_jacDiagABInv(0.f), + :m_appliedImpulse(btScalar(0.)), + m_prevAppliedImpulse(btScalar(0.)), + m_accumulatedTangentImpulse0(btScalar(0.)), + m_accumulatedTangentImpulse1(btScalar(0.)), + m_jacDiagABInv(btScalar(0.)), m_persistentLifeTime(0), - m_restitution(0.f), - m_friction(0.f), - m_penetration(0.f), + m_restitution(btScalar(0.)), + m_friction(btScalar(0.)), + m_penetration(btScalar(0.)), m_contactSolverFunc(0), m_frictionSolverFunc(0) { @@ -58,18 +58,18 @@ struct btConstraintPersistentData /// total applied impulse during most recent frame - float m_appliedImpulse; - float m_prevAppliedImpulse; - float m_accumulatedTangentImpulse0; - float m_accumulatedTangentImpulse1; + btScalar m_appliedImpulse; + btScalar m_prevAppliedImpulse; + btScalar m_accumulatedTangentImpulse0; + btScalar m_accumulatedTangentImpulse1; - float m_jacDiagABInv; - float m_jacDiagABInvTangent0; - float m_jacDiagABInvTangent1; + btScalar m_jacDiagABInv; + btScalar m_jacDiagABInvTangent0; + btScalar m_jacDiagABInvTangent1; int m_persistentLifeTime; - float m_restitution; - float m_friction; - float m_penetration; + btScalar m_restitution; + btScalar m_friction; + btScalar m_penetration; btVector3 m_frictionWorldTangential0; btVector3 m_frictionWorldTangential1; @@ -91,19 +91,28 @@ struct btConstraintPersistentData ///positive distance = separation, negative distance = penetration void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep); + btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep); ///contact constraint resolution: ///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint ///positive distance = separation, negative distance = penetration -float resolveSingleCollision( +btScalar resolveSingleCollision( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& info); -float resolveSingleFriction( +btScalar resolveSingleFriction( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo + ); + + + +btScalar resolveSingleCollisionCombined( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index ed1ba6ac1ba..c3c73e300f4 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -22,25 +22,25 @@ struct btContactSolverInfo inline btContactSolverInfo() { - m_tau = 0.6f; - m_damping = 1.0f; - m_friction = 0.3f; - m_restitution = 0.f; - m_maxErrorReduction = 20.f; + m_tau = btScalar(0.6); + m_damping = btScalar(1.0); + m_friction = btScalar(0.3); + m_restitution = btScalar(0.); + m_maxErrorReduction = btScalar(20.); m_numIterations = 10; - m_erp = 0.4f; - m_sor = 1.3f; + m_erp = btScalar(0.4); + m_sor = btScalar(1.3); } - float m_tau; - float m_damping; - float m_friction; - float m_timeStep; - float m_restitution; + btScalar m_tau; + btScalar m_damping; + btScalar m_friction; + btScalar m_timeStep; + btScalar m_restitution; int m_numIterations; - float m_maxErrorReduction; - float m_sor; - float m_erp; + btScalar m_maxErrorReduction; + btScalar m_sor; + btScalar m_erp; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index b2132a8d4f3..747d10d1f8b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -17,8 +17,9 @@ subject to the following restrictions: #include "btGeneric6DofConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" +#include -static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f }; +static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) }; static const int kAxisA[] = { 1, 0, 0 }; static const int kAxisB[] = { 2, 2, 1 }; #define GENERIC_D6_DISABLE_WARMSTARTING 1 @@ -38,9 +39,9 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& //so start all locked for (int i=0; i<6;++i) { - m_lowerLimit[i] = 0.0f; - m_upperLimit[i] = 0.0f; - m_accumulatedImpulse[i] = 0.0f; + m_lowerLimit[i] = btScalar(0.0); + m_upperLimit[i] = btScalar(0.0); + m_accumulatedImpulse[i] = btScalar(0.0); } } @@ -83,7 +84,7 @@ void btGeneric6DofConstraint::buildJacobian() //optionally disable warmstarting #ifdef GENERIC_D6_DISABLE_WARMSTARTING - m_accumulatedImpulse[i] = 0.f; + m_accumulatedImpulse[i] = btScalar(0.); #endif //GENERIC_D6_DISABLE_WARMSTARTING // Apply accumulated impulse @@ -115,7 +116,7 @@ void btGeneric6DofConstraint::buildJacobian() m_rbB.getInvInertiaDiagLocal()); #ifdef GENERIC_D6_DISABLE_WARMSTARTING - m_accumulatedImpulse[i + 3] = 0.f; + m_accumulatedImpulse[i + 3] = btScalar(0.); #endif //GENERIC_D6_DISABLE_WARMSTARTING // Apply accumulated impulse @@ -127,7 +128,7 @@ void btGeneric6DofConstraint::buildJacobian() } } -float getMatrixElem(const btMatrix3x3& mat,int index) +btScalar getMatrixElem(const btMatrix3x3& mat,int index) { int row = index%3; int col = index / 3; @@ -143,9 +144,9 @@ bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) /// 0..8 - if (getMatrixElem(mat,2) < 1.0f) + if (getMatrixElem(mat,2) < btScalar(1.0)) { - if (getMatrixElem(mat,2) > -1.0f) + if (getMatrixElem(mat,2) > btScalar(-1.0)) { xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8)); xyz[1] = btAsin(getMatrixElem(mat,2)); @@ -157,7 +158,7 @@ bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) // 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; + xyz[2] = btScalar(0.0); return false; } } @@ -167,16 +168,17 @@ bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); xyz[1] = SIMD_HALF_PI; xyz[2] = 0.0; - return false; + } - return false; + + return false; } void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) { - btScalar tau = 0.1f; - btScalar damping = 1.0f; + btScalar tau = btScalar(0.1); + btScalar damping = btScalar(1.0); btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); @@ -199,7 +201,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) localNormalInA[i] = 1; btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA; - btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); //velocity error (first order error) btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, @@ -207,8 +209,8 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); - btScalar lo = -1e30f; - btScalar hi = 1e30f; + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); //handle the limits if (m_lowerLimit[i] < m_upperLimit[i]) @@ -217,14 +219,14 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) if (depth > m_upperLimit[i]) { depth -= m_upperLimit[i]; - lo = 0.f; + lo = btScalar(0.); } else { if (depth < m_lowerLimit[i]) { depth -= m_lowerLimit[i]; - hi = 0.f; + hi = btScalar(0.); } else { continue; @@ -234,9 +236,9 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) } 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; + btScalar oldNormalImpulse = m_accumulatedImpulse[i]; + btScalar sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[i] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse; btVector3 impulse_vector = normalWorld * normalImpulse; @@ -267,7 +269,7 @@ 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(); - btScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); //velocity error (first order error) btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, @@ -279,27 +281,27 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btScalar rel_pos = kSign[i] * axisA.dot(axisB); - btScalar lo = -1e30f; - btScalar hi = 1e30f; + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); //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; + btScalar loLimit = m_lowerLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : btScalar(-1e30); + btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : btScalar(1e30); - float projAngle = -2.f*xyz[i]; + btScalar projAngle = btScalar(-1.)*xyz[i]; if (projAngle < loLimit) { - hi = 0.f; + hi = btScalar(0.); rel_pos = (loLimit - projAngle); } else { if (projAngle > hiLimit) { - lo = 0.f; + lo = btScalar(0.); rel_pos = (hiLimit - projAngle); } else { @@ -311,9 +313,9 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) //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; + btScalar oldNormalImpulse = m_accumulatedImpulse[i+3]; + btScalar sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[i+3] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse; // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) @@ -328,12 +330,13 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { + (void)timeStep; } btScalar btGeneric6DofConstraint::computeAngle(int axis) const { - btScalar angle; + btScalar angle = btScalar(0.f); switch (axis) { @@ -375,9 +378,12 @@ btScalar btGeneric6DofConstraint::computeAngle(int axis) const angle = btAtan2( s, c ); } break; - default: assert ( 0 ) ; break ; + default: + btAssert ( 0 ) ; + + break ; } - return angle; + return angle; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 329048b5737..b114e54fa69 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef GENERIC_6DOF_CONSTRAINT_H #define GENERIC_6DOF_CONSTRAINT_H -#include "LinearMath/btVector3.h" - -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "../../LinearMath/btVector3.h" +#include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; @@ -41,12 +40,19 @@ class btGeneric6DofConstraint : public btTypedConstraint btScalar m_accumulatedImpulse[6]; + btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) + { + btAssert(0); + (void) other; + return *this; + } public: btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ); btGeneric6DofConstraint(); + virtual void buildJacobian(); virtual void solveConstraint(btScalar timeStep); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index f72278e2cbf..27e30987549 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -17,7 +17,7 @@ subject to the following restrictions: #include "btHingeConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" - +#include btHingeConstraint::btHingeConstraint(): m_enableAngularMotor(false) @@ -49,7 +49,7 @@ m_enableAngularMotor(false) void btHingeConstraint::buildJacobian() { - m_appliedImpulse = 0.f; + m_appliedImpulse = btScalar(0.); btVector3 normal(0,0,0); @@ -115,8 +115,8 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; btVector3 normal(0,0,0); - btScalar tau = 0.3f; - btScalar damping = 1.f; + btScalar tau = btScalar(0.3); + btScalar damping = btScalar(1.); //linear part if (!m_angularOnly) @@ -124,7 +124,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) for (int i=0;i<3;i++) { normal[i] = 1; - btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); @@ -165,27 +165,27 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btVector3 velrelOrthog = angAorthog-angBorthog; { //solve orthogonal angular velocity correction - float relaxation = 1.f; - float len = velrelOrthog.length(); - if (len > 0.00001f) + btScalar relaxation = btScalar(1.); + btScalar len = velrelOrthog.length(); + if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); - float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + getRigidBodyB().computeAngularImpulseDenominator(normal); // scale for mass and relaxation //todo: expose this 0.9 factor to developer - velrelOrthog *= (1.f/denom) * 0.9f; + velrelOrthog *= (btScalar(1.)/denom) * btScalar(0.9); } //solve angular positional correction - btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); - float len2 = angularError.length(); - if (len2>0.00001f) + btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); - float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + getRigidBodyB().computeAngularImpulseDenominator(normal2); - angularError *= (1.f/denom2) * relaxation; + angularError *= (btScalar(1.)/denom2) * relaxation; } m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); @@ -204,10 +204,10 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btScalar desiredMotorVel = m_motorTargetVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; - float denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) + + btScalar denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) + getRigidBodyB().computeAngularImpulseDenominator(axisA); - btScalar unclippedMotorImpulse = (1.f/denom3) * motor_relvel;; + btScalar unclippedMotorImpulse = (btScalar(1.)/denom3) * motor_relvel;; //todo: should clip against accumulated impulse btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; @@ -223,6 +223,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) void btHingeConstraint::updateRHS(btScalar timeStep) { + (void)timeStep; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 553ec135c8a..5c1ceafbc5b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef HINGECONSTRAINT_H #define HINGECONSTRAINT_H -#include "LinearMath/btVector3.h" - -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "../../LinearMath/btVector3.h" +#include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; @@ -38,8 +37,8 @@ class btHingeConstraint : public btTypedConstraint bool m_angularOnly; - float m_motorTargetVelocity; - float m_maxMotorImpulse; + btScalar m_motorTargetVelocity; + btScalar m_maxMotorImpulse; bool m_enableAngularMotor; public: @@ -70,7 +69,7 @@ public: m_angularOnly = angularOnly; } - void enableAngularMotor(bool enableMotor,float targetVelocity,float maxMotorImpulse) + void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse) { m_enableAngularMotor = enableMotor; m_motorTargetVelocity = targetVelocity; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index 384e4f7bab5..aae3ed0373f 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef JACOBIAN_ENTRY_H #define JACOBIAN_ENTRY_H -#include "LinearMath/btVector3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "../../LinearMath/btVector3.h" +#include "../Dynamics/btRigidBody.h" //notes: @@ -50,7 +50,7 @@ public: m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } //angular constraint between two different rigidbodies @@ -59,7 +59,7 @@ public: const btMatrix3x3& world2B, const btVector3& inertiaInvA, const btVector3& inertiaInvB) - :m_linearJointAxis(btVector3(0.f,0.f,0.f)) + :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) { m_aJ= world2A*jointAxis; m_bJ = world2B*-jointAxis; @@ -67,7 +67,7 @@ public: m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } //angular constraint between two different rigidbodies @@ -75,7 +75,7 @@ public: const btVector3& axisInB, const btVector3& inertiaInvA, const btVector3& inertiaInvB) - : m_linearJointAxis(btVector3(0.f,0.f,0.f)) + : m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) , m_aJ(axisInA) , m_bJ(-axisInB) { @@ -83,7 +83,7 @@ public: m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } //constraint on one rigidbody @@ -98,10 +98,10 @@ public: m_aJ= world2A*(rel_pos1.cross(jointAxis)); m_bJ = world2A*(rel_pos2.cross(-jointAxis)); m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = btVector3(0.f,0.f,0.f); + m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } btScalar getDiagonal() const { return m_Adiag; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index d15bdaad790..aacb0a3ea66 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btPoint2PointConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" - +#include @@ -39,7 +39,7 @@ btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector void btPoint2PointConstraint::buildJacobian() { - m_appliedImpulse = 0.f; + m_appliedImpulse = btScalar(0.); btVector3 normal(0,0,0); @@ -76,7 +76,7 @@ void btPoint2PointConstraint::solveConstraint(btScalar timeStep) for (int i=0;i<3;i++) { normal[i] = 1; - btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); @@ -110,6 +110,7 @@ void btPoint2PointConstraint::solveConstraint(btScalar timeStep) void btPoint2PointConstraint::updateRHS(btScalar timeStep) { + (void)timeStep; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index 8aae8d74ce7..71da8ac0347 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef POINT2POINTCONSTRAINT_H #define POINT2POINTCONSTRAINT_H -#include "LinearMath/btVector3.h" - -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "../../LinearMath/btVector3.h" +#include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; @@ -26,12 +25,12 @@ class btRigidBody; struct btConstraintSetting { btConstraintSetting() : - m_tau(0.3f), - m_damping(1.f) + m_tau(btScalar(0.3)), + m_damping(btScalar(1.)) { } - float m_tau; - float m_damping; + btScalar m_tau; + btScalar m_damping; }; /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index eaf172b9395..4366284ea73 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -24,7 +24,13 @@ subject to the following restrictions: #include "btJacobianEntry.h" #include "LinearMath/btMinMax.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include +#include "LinearMath/btStackAlloc.h" +#include "LinearMath/btQuickprof.h" +#include "btSolverBody.h" +#include "btSolverConstraint.h" +#include "LinearMath/btAlignedObjectArray.h" #ifdef USE_PROFILE #include "LinearMath/btQuickprof.h" @@ -36,24 +42,26 @@ int gTotalContactPoints = 0; struct btOrderIndex { - short int m_manifoldIndex; - short int m_pointIndex; + int m_manifoldIndex; + 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() + + +unsigned long btSequentialImpulseConstraintSolver::btRand2() { - btSeed2 = (1664525L*btSeed2 + 1013904223L) & 0xffffffff; - return btSeed2; + m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; + return m_btSeed2; } - //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) -int btRandInt2 (int n) +int btSequentialImpulseConstraintSolver::btRandInt2 (int n) { // seems good; xor-fold and modulus const unsigned long un = n; @@ -82,15 +90,7 @@ int btRandInt2 (int n) -int btRandIntWrong (int n) -{ - float a = float(n) / 4294967296.0f; -// printf("n = %d\n",n); -// printf("a = %f\n",a); - int res = (int) (float(btRand2()) * a); -// printf("res=%d\n",res); - return res; -} + bool MyContactDestroyedCallback(void* userPersistentData) { @@ -102,18 +102,12 @@ bool MyContactDestroyedCallback(void* userPersistentData) return true; } -btSequentialImpulseConstraintSolver3::btSequentialImpulseConstraintSolver3() -{ - btSeed2 = 0; - setSolverMode(SOLVER_RANDMIZE_ORDER); -} btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_solverMode(SOLVER_USE_WARMSTARTING) +:m_solverMode(SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY), //not using SOLVER_USE_WARMSTARTING, +m_btSeed2(0) { - btSeed2 = 0; - gContactDestroyedCallback = &MyContactDestroyedCallback; //initialize default friction/contact funcs @@ -127,35 +121,487 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() } } -/// btSequentialImpulseConstraintSolver Sequentially applies impulses -float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) + +void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody) +{ +/* int size = sizeof(btSolverBody); + int sizeofrb = sizeof(btRigidBody); + int sizemanifold = sizeof(btPersistentManifold); + int sizeofmp = sizeof(btManifoldPoint); + int sizeofPersistData = sizeof (btConstraintPersistentData); +*/ + + solverBody->m_angularVelocity = rigidbody->getAngularVelocity(); + solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition(); + solverBody->m_friction = rigidbody->getFriction(); +// solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld(); + solverBody->m_invMass = rigidbody->getInvMass(); + solverBody->m_linearVelocity = rigidbody->getLinearVelocity(); + solverBody->m_originalBody = rigidbody; + solverBody->m_angularFactor = rigidbody->getAngularFactor(); +} + +btScalar penetrationResolveFactor = btScalar(0.9); +btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) +{ + btScalar rest = restitution * -rel_vel; + return rest; +} + + + + + + +//velocity + friction +//response between two dynamic objects with friction +SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo) +{ + (void)solverInfo; + + btScalar normalImpulse(0.f); + { + if (contactConstraint.m_penetration < 0.f) + return 0.f; + + // Optimized version of projected relative velocity, use precomputed cross products with normal + // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); + // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); + // btVector3 vel = vel1 - vel2; + // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); + + btScalar rel_vel; + btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) + + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); + btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) + + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); + + rel_vel = vel1Dotn-vel2Dotn; + + + btScalar positionalError = contactConstraint.m_penetration; + btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping; + + btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv; + btScalar normalImpulse = penetrationImpulse+velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; + + btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse; + btScalar velocitySum = oldVelocityImpulse + velocityImpulse; + contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum; + + normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse; + + if (body1.m_invMass) + { + body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass, + contactConstraint.m_angularComponentA,normalImpulse); + } + if (body2.m_invMass) + { + body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass, + contactConstraint.m_angularComponentB,-normalImpulse); + } + + } + + + + return normalImpulse; +} + + +#ifndef NO_FRICTION_TANGENTIALS + +SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo, + btScalar appliedNormalImpulse) { + (void)solverInfo; + - btContactSolverInfo info = infoGlobal; + const btScalar combinedFriction = contactConstraint.m_friction; + + const btScalar limit = appliedNormalImpulse * combinedFriction; + + if (appliedNormalImpulse>btScalar(0.)) + //friction + { + + btScalar j1; + { - int numiter = infoGlobal.m_numIterations; -#ifdef USE_PROFILE - btProfiler::beginBlock("solve"); -#endif //USE_PROFILE + btScalar rel_vel; + const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) + + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); + const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) + + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); + rel_vel = vel1Dotn-vel2Dotn; + + // calculate j that moves us to zero relative velocity + j1 = -rel_vel * contactConstraint.m_jacDiagABInv; + btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse; + contactConstraint.m_appliedImpulse = oldTangentImpulse + j1; + GEN_set_min(contactConstraint.m_appliedImpulse, limit); + GEN_set_max(contactConstraint.m_appliedImpulse, -limit); + j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse; - int totalPoints = 0; + } + + if (body1.m_invMass) + { + body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1); + } + if (body2.m_invMass) + { + body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1); + } + + } + return 0.f; +} +#else + +//velocity + friction +//response between two dynamic objects with friction +btScalar resolveSingleFrictionCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo) +{ + + btVector3 vel1; + btVector3 vel2; + btScalar normalImpulse(0.f); + { - int j; - for (j=0;j 0.f) + if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON) { - btPersistentManifold* manifold = manifoldPtr[j]; - prepareConstraints(manifold,info,debugDrawer); + lat_rel_vel = btSqrt(lat_rel_vel); + + lat_vel /= lat_rel_vel; + btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel); + btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel); + btScalar friction_impulse = lat_rel_vel / + (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); + btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction; + + GEN_set_min(friction_impulse, normal_impulse); + GEN_set_max(friction_impulse, -normal_impulse); + body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1); + body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); + } + } + + return normalImpulse; +} + +#endif //NO_FRICTION_TANGENTIALS + +btAlignedObjectArray tmpSolverBodyPool; +btAlignedObjectArray tmpSolverConstraintPool; +btAlignedObjectArray tmpSolverFrictionConstraintPool; + + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +{ + (void)stackAlloc; + (void)debugDrawer; + + if (!(numConstraints + numManifolds)) + { +// printf("empty\n"); + return 0.f; + } + + BEGIN_PROFILE("refreshManifolds"); + + int i; + for (i=0;igetBody0(); + btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); - for (int p=0;pgetNumContacts();p++) + manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); + + } + + END_PROFILE("refreshManifolds"); + + + BEGIN_PROFILE("gatherSolverData"); + + //int sizeofSB = sizeof(btSolverBody); + //int sizeofSC = sizeof(btSolverConstraint); + + + //if (1) + { + //if m_stackAlloc, try to pack bodies/constraints to speed up solving +// btBlock* sablock; +// sablock = stackAlloc->beginBlock(); + + // int memsize = 16; +// unsigned char* stackMemory = stackAlloc->allocate(memsize); + + + //todo: use stack allocator for this temp memory + int minReservation = numManifolds*2; + + tmpSolverBodyPool.reserve(minReservation); + + { + for (int i=0;igetIslandTag() >= 0)) + { + btAssert(rb->getCompanionId() < 0); + int solverBodyId = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb); + rb->setCompanionId(solverBodyId); + } + } + } + + + tmpSolverConstraintPool.reserve(minReservation); + tmpSolverFrictionConstraintPool.reserve(minReservation); + { + int i; + + for (i=0;igetBody0(); + btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); + + + int solverBodyIdA=-1; + int solverBodyIdB=-1; + + if (manifold->getNumContacts()) + { + + + + if (rb0->getIslandTag() >= 0) + { + solverBodyIdA = rb0->getCompanionId(); + } else + { + //create a static body + solverBodyIdA = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb0); + } + + if (rb1->getIslandTag() >= 0) + { + solverBodyIdB = rb1->getCompanionId(); + } else + { + //create a static body + solverBodyIdB = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb1); + } + } + + for (int j=0;jgetNumContacts();j++) + { + + btManifoldPoint& cp = manifold->getContactPoint(j); + + int frictionIndex = tmpSolverConstraintPool.size(); + + if (cp.getDistance() <= btScalar(0.)) + { + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition(); + + + btScalar relaxation = 1.f; + + { + btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand(); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; + + + + { + //can be optimized, the cross products are already calculated + btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + } + + solverConstraint.m_contactNormal = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB); + + + btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2); + + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + + solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations); + solverConstraint.m_friction = cp.m_combinedFriction; + btScalar rest = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (rest <= btScalar(0.)) + { + rest = 0.f; + }; + + btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep; + if (rest > penVel) + { + rest = btScalar(0.); + } + solverConstraint.m_restitution = rest; + + solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep); + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedVelocityImpulse = 0.f; + + + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0; + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1; + } + + //create 2 '1d axis' constraints for 2 tangential friction directions + + //re-calculate friction direction every frame, todo: check if this is really needed + btVector3 frictionTangential0a, frictionTangential1b; + + btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b); + + { + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); + solverConstraint.m_contactNormal = frictionTangential0a; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + + { + btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis0; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0; + } + { + btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis0; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0; + } + + } + + + { + + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); + solverConstraint.m_contactNormal = frictionTangential1b; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + { + btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1; + } + { + btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1; + } + } + + } + } } } } + END_PROFILE("gatherSolverData"); + + BEGIN_PROFILE("prepareConstraints"); + + btContactSolverInfo info = infoGlobal; { int j; @@ -166,21 +612,57 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma } } - //should traverse the contacts random order... - int iteration; + btAlignedObjectArray gOrderTmpConstraintPool; + btAlignedObjectArray gOrderFrictionConstraintPool; + int numConstraintPool = tmpSolverConstraintPool.size(); + int numFrictionPool = tmpSolverFrictionConstraintPool.size(); + + ///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + gOrderTmpConstraintPool.resize(numConstraintPool); + gOrderFrictionConstraintPool.resize(numFrictionPool); { - for ( iteration = 0;iterationgetRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity(); + } + if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity(); + } + constraint->solveConstraint(info.m_timeStep); + + if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity(); + } + if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity(); + } + } - for (j=0;jgetBody0(), - (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + int numPoolConstraints = tmpSolverConstraintPool.size(); + for (j=0;jgetBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size(); + for (j=0;jgetNumContacts();p++) + + for (short p=0;pgetNumContacts();p++) { - //interleaving here gives better results - solve( (btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1() - ,manifoldPtr[j]->getContactPoint(p),info,0,debugDrawer); + gOrder[totalPoints].m_manifoldIndex = j; + gOrder[totalPoints].m_pointIndex = p; + totalPoints++; } } } + { int j; for (j=0;jsolveConstraint(info.m_timeStep); - } + int j; + if (m_solverMode & SOLVER_RANDMIZE_ORDER) + { + if ((iteration & 7) == 0) { + for (j=0; jgetNumContacts();p++) + for (j=0;jsolveConstraint(info.m_timeStep); + } + + for (j=0;jgetBody0(), (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(p),info,iteration,debugDrawer); + ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); } - } - - } - - for ( iteration = 0;iterationgetNumContacts();p++) + + for (j=0;jgetBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(p),info,iteration,debugDrawer); + (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); } } } - + END_PROFILE("solveConstraints"); + + #ifdef USE_PROFILE btProfiler::endBlock("solve"); #endif //USE_PROFILE - return 0.f; -} -float penetrationResolveFactor = 0.9f; -btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) -{ - btScalar rest = restitution * -rel_vel; - return rest; + + return btScalar(0.); } + + + + + void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer) { + (void)debugDrawer; + btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); @@ -327,7 +884,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol for (int i=0;igetContactPoint(i); - if (cp.getDistance() <= 0.f) + if (cp.getDistance() <= btScalar(0.)) { const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -376,7 +933,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol } assert(cpd); - cpd->m_jacDiagABInv = 1.f / jacDiagAB; + cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB; //Dependent on Rigidbody A and B types, fetch the contact/friction response func //perhaps do a similar thing for friction/restutution combiner funcs... @@ -390,14 +947,14 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol btScalar rel_vel; rel_vel = cp.m_normalWorldOnB.dot(vel); - float combinedRestitution = cp.m_combinedRestitution; + btScalar combinedRestitution = cp.m_combinedRestitution; - cpd->m_penetration = cp.getDistance(); + cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations); cpd->m_friction = cp.m_combinedFriction; cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); - if (cpd->m_restitution <= 0.) //0.f) + if (cpd->m_restitution <= btScalar(0.)) { - cpd->m_restitution = 0.0f; + cpd->m_restitution = btScalar(0.0); }; @@ -408,18 +965,18 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol if (cpd->m_restitution > penVel) { - cpd->m_penetration = 0.f; + cpd->m_penetration = btScalar(0.); } - float relaxation = info.m_damping; + btScalar relaxation = info.m_damping; if (m_solverMode & SOLVER_USE_WARMSTARTING) { cpd->m_appliedImpulse *= relaxation; } else { - cpd->m_appliedImpulse =0.f; + cpd->m_appliedImpulse =btScalar(0.); } //for friction @@ -432,12 +989,12 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol #define NO_FRICTION_WARMSTART 1 #ifdef NO_FRICTION_WARMSTART - cpd->m_accumulatedTangentImpulse0 = 0.f; - cpd->m_accumulatedTangentImpulse1 = 0.f; + cpd->m_accumulatedTangentImpulse0 = btScalar(0.); + cpd->m_accumulatedTangentImpulse1 = btScalar(0.); #endif //NO_FRICTION_WARMSTART - float denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); - float denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); - float denom = relaxation/(denom0+denom1); + btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); + btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); + btScalar denom = relaxation/(denom0+denom1); cpd->m_jacDiagABInvTangent0 = denom; @@ -492,16 +1049,54 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol } } -float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) + +btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) { + btScalar maxImpulse = btScalar(0.); + + { + + btVector3 color(0,1,0); + { + if (cp.getDistance() <= btScalar(0.)) + { - float maxImpulse = 0.f; + if (iter == 0) + { + if (debugDrawer) + debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); + } + + { + + //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + btScalar impulse = resolveSingleCollisionCombined( + *body0,*body1, + cp, + info); + + if (maxImpulse < impulse) + maxImpulse = impulse; + + } + } + } + } + return maxImpulse; +} + + + +btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +{ + + btScalar maxImpulse = btScalar(0.); { btVector3 color(0,1,0); { - if (cp.getDistance() <= 0.f) + if (cp.getDistance() <= btScalar(0.)) { if (iter == 0) @@ -513,7 +1108,7 @@ float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* { btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - float impulse = cpd->m_contactSolverFunc( + btScalar impulse = cpd->m_contactSolverFunc( *body0,*body1, cp, info); @@ -528,16 +1123,19 @@ float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* return maxImpulse; } -float btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) { + (void)debugDrawer; + (void)iter; + { btVector3 color(0,1,0); { - if (cp.getDistance() <= 0.f) + if (cp.getDistance() <= btScalar(0.)) { btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; @@ -552,5 +1150,5 @@ float btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRi } - return 0.f; + return btScalar(0.); } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 0989a86e2cd..13e70c41be4 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -18,7 +18,6 @@ subject to the following restrictions: #include "btConstraintSolver.h" class btIDebugDraw; - #include "btContactConstraint.h" @@ -31,8 +30,8 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver { protected: - 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); + btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + btScalar 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]; @@ -40,6 +39,8 @@ protected: //choose between several modes, different friction model etc. int m_solverMode; + ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction + unsigned long m_btSeed2; public: @@ -47,7 +48,8 @@ public: { SOLVER_RANDMIZE_ORDER = 1, SOLVER_FRICTION_SEPARATE = 2, - SOLVER_USE_WARMSTARTING = 4 + SOLVER_USE_WARMSTARTING = 4, + SOLVER_CACHE_FRIENDLY = 8 }; btSequentialImpulseConstraintSolver(); @@ -68,7 +70,12 @@ public: virtual ~btSequentialImpulseConstraintSolver() {} - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc); + + virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + + btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + void setSolverMode(int mode) { @@ -79,20 +86,24 @@ public: { return m_solverMode; } -}; - -/// Small variation on btSequentialImpulseConstraintSolver: warmstarting, separate friction, non-randomized ordering -class btSequentialImpulseConstraintSolver3 : public btSequentialImpulseConstraintSolver -{ -public: - btSequentialImpulseConstraintSolver3(); + unsigned long btRand2(); - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + int btRandInt2 (int n); + void setRandSeed(unsigned long seed) + { + m_btSeed2 = seed; + } + unsigned long getRandSeed() const + { + return m_btSeed2; + } }; + + #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp index edca6c8fa08..0c7dbd668bb 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @@ -43,12 +43,18 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( btScalar depthB, const btVector3& normalB, btScalar& imp0,btScalar& imp1) { + (void)linvelA; + (void)linvelB; + (void)angvelB; + (void)angvelA; - imp0 = 0.f; - imp1 = 0.f; - btScalar len = fabs(normalA.length())-1.f; - if (fabs(len) >= SIMD_EPSILON) + + imp0 = btScalar(0.); + imp1 = btScalar(0.); + + btScalar len = btFabs(normalA.length()) - btScalar(1.); + if (btFabs(len) >= SIMD_EPSILON) return; btAssert(len < SIMD_EPSILON); @@ -67,7 +73,7 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv - btScalar massTerm = 1.f / (invMassA + invMassB); + btScalar massTerm = btScalar(1.) / (invMassA + invMassB); // calculate rhs (or error) terms @@ -87,7 +93,7 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); + btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; @@ -126,11 +132,18 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( btScalar& imp0,btScalar& imp1) { - imp0 = 0.f; - imp1 = 0.f; + (void)linvelA; + (void)linvelB; + (void)angvelA; + (void)angvelB; + + - btScalar len = fabs(normalA.length())-1.f; - if (fabs(len) >= SIMD_EPSILON) + imp0 = btScalar(0.); + imp1 = btScalar(0.); + + btScalar len = btFabs(normalA.length()) - btScalar(1.); + if (btFabs(len) >= SIMD_EPSILON) return; btAssert(len < SIMD_EPSILON); @@ -164,7 +177,7 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); + btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; @@ -178,41 +191,41 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( //[jA nD] * [imp0] = [dv0] //[nD jB] [imp1] [dv1] - if ( imp0 > 0.0f) + if ( imp0 > btScalar(0.0)) { - if ( imp1 > 0.0f ) + if ( imp1 > btScalar(0.0) ) { //both positive } else { - imp1 = 0.f; + imp1 = btScalar(0.); // now imp0>0 imp1<0 imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > 0.0f ) + if ( imp0 > btScalar(0.0) ) { } else { - imp0 = 0.f; + imp0 = btScalar(0.); } } } else { - imp0 = 0.f; + imp0 = btScalar(0.); imp1 = dv1 / jacB.getDiagonal(); - if ( imp1 <= 0.0f ) + if ( imp1 <= btScalar(0.0) ) { - imp1 = 0.f; + imp1 = btScalar(0.); // now imp0>0 imp1<0 imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > 0.0f ) + if ( imp0 > btScalar(0.0) ) { } else { - imp0 = 0.f; + imp0 = btScalar(0.); } } else { @@ -221,7 +234,7 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( } - +/* void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, const btScalar invMassA, const btVector3& linvelA,const btVector3& angvelA, @@ -238,4 +251,5 @@ void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invI { } +*/ diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h index 639e4c9433f..e7d26645c6a 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef SOLVE_2LINEAR_CONSTRAINT_H #define SOLVE_2LINEAR_CONSTRAINT_H -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "../../LinearMath/btVector3.h" class btRigidBody; @@ -85,7 +85,7 @@ public: btScalar depthB, const btVector3& normalB, btScalar& imp0,btScalar& imp1); - +/* void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, const btScalar invMassA, const btVector3& linvelA,const btVector3& angvelA, @@ -100,6 +100,7 @@ public: btScalar depthB, const btVector3& normalB, btScalar& imp0,btScalar& imp1); +*/ }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h new file mode 100644 index 00000000000..0ab536f42b3 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -0,0 +1,71 @@ +/* +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_SOLVER_BODY_H +#define BT_SOLVER_BODY_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" + + + + +ATTRIBUTE_ALIGNED16 (struct) btSolverBody +{ + btVector3 m_centerOfMassPosition; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btRigidBody* m_originalBody; + float m_invMass; + float m_friction; + float m_angularFactor; + + inline void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + { + velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos); + } + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + m_linearVelocity += linearComponent*impulseMagnitude; + m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor; + } + + void writebackVelocity() + { + if (m_invMass) + { + m_originalBody->setLinearVelocity(m_linearVelocity); + m_originalBody->setAngularVelocity(m_angularVelocity); + } + } + + void readVelocity() + { + if (m_invMass) + { + m_linearVelocity = m_originalBody->getLinearVelocity(); + m_angularVelocity = m_originalBody->getAngularVelocity(); + } + } + + + + +}; + +#endif //BT_SOLVER_BODY_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h new file mode 100644 index 00000000000..f1f40ffdf19 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -0,0 +1,63 @@ + + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOLVER_CONSTRAINT_H +#define BT_SOLVER_CONSTRAINT_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" + +//#define NO_FRICTION_TANGENTIALS 1 + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint +{ + btVector3 m_relpos1CrossNormal; + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal; + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + btScalar m_appliedVelocityImpulse; + int m_solverBodyIdA; + int m_solverBodyIdB; + btScalar m_friction; + btScalar m_restitution; + btScalar m_jacDiagABInv; + btScalar m_penetration; + btScalar m_appliedImpulse; + + int m_constraintType; + int m_frictionIndex; + int m_unusedPadding[2]; + + enum btSolverConstraintType + { + BT_SOLVER_CONTACT_1D = 0, + BT_SOLVER_FRICTION_1D + }; +}; + + + + + + +#endif //BT_SOLVER_CONSTRAINT_H + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index cb3fa72d440..a15b3e026cd 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -24,18 +24,18 @@ btTypedConstraint::btTypedConstraint() m_userConstraintId(-1), m_rbA(s_fixed), m_rbB(s_fixed), -m_appliedImpulse(0.f) +m_appliedImpulse(btScalar(0.)) { - s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } btTypedConstraint::btTypedConstraint(btRigidBody& rbA) : m_userConstraintType(-1), m_userConstraintId(-1), m_rbA(rbA), m_rbB(s_fixed), -m_appliedImpulse(0.f) +m_appliedImpulse(btScalar(0.)) { - s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } @@ -45,9 +45,9 @@ btTypedConstraint::btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB) m_userConstraintId(-1), m_rbA(rbA), m_rbB(rbB), -m_appliedImpulse(0.f) +m_appliedImpulse(btScalar(0.)) { - s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index bc25eaa759c..dfee6e80d0e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -17,19 +17,25 @@ subject to the following restrictions: #define TYPED_CONSTRAINT_H class btRigidBody; -#include "LinearMath/btScalar.h" +#include "../../LinearMath/btScalar.h" ///TypedConstraint is the baseclass for Bullet constraints and vehicles class btTypedConstraint { int m_userConstraintType; int m_userConstraintId; - + + btTypedConstraint& operator=(btTypedConstraint& other) + { + btAssert(0); + (void) other; + return *this; + } protected: btRigidBody& m_rbA; btRigidBody& m_rbB; - float m_appliedImpulse; + btScalar m_appliedImpulse; public: @@ -81,7 +87,7 @@ public: { return m_userConstraintId; } - float getAppliedImpulse() + btScalar getAppliedImpulse() { return m_appliedImpulse; } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index fc37cf2e3c3..f80a0826fbf 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -16,7 +16,6 @@ subject to the following restrictions: #include "btDiscreteDynamicsWorld.h" - //collision detection #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" @@ -54,8 +53,6 @@ subject to the following restrictions: -#include - btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) @@ -63,7 +60,7 @@ btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOver m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver), m_debugDrawer(0), m_gravity(0,-10,0), -m_localTime(1.f/60.f), +m_localTime(btScalar(1.)/btScalar(60.)), m_profileTimings(0) { m_islandManager = new btSimulationIslandManager(); @@ -81,10 +78,10 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() delete m_constraintSolver; } -void btDiscreteDynamicsWorld::saveKinematicState(float timeStep) +void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) { - for (unsigned int i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) { - btVector3 color(255.f,255.f,255.f); + btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.)); switch(colObj->getActivationState()) { case ACTIVE_TAG: - color = btVector3(255.f,255.f,255.f); break; + color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break; case ISLAND_SLEEPING: - color = btVector3(0.f,255.f,0.f);break; + color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break; case WANTS_DEACTIVATION: - color = btVector3(0.f,255.f,255.f);break; + color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break; case DISABLE_DEACTIVATION: - color = btVector3(255.f,0.f,0.f);break; + color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break; case DISABLE_SIMULATION: - color = btVector3(255.f,255.f,0.f);break; + color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break; default: { - color = btVector3(255.f,0.f,0.f); + color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.)); } }; @@ -139,7 +136,10 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() btRigidBody* body = btRigidBody::upcast(colObj); if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) { - if (body->getActivationState() != ISLAND_SLEEPING) + //we need to call the update at least once, even for sleeping objects + //otherwise the 'graphics' transform never updates properly + //so todo: add 'dirty' flag + //if (body->getActivationState() != ISLAND_SLEEPING) { btTransform interpolatedTransform; btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), @@ -152,7 +152,7 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) { - for (unsigned int i=0;im_vehicles.size();i++) + for ( int i=0;im_vehicles.size();i++) { for (int v=0;vgetNumWheels();v++) { @@ -188,7 +188,7 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() } -int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, float fixedTimeStep) +int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) { int numSimulationSubSteps = 0; @@ -243,7 +243,7 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo return numSimulationSubSteps; } -void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep) +void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { startProfiling(timeStep); @@ -291,7 +291,7 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep) void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for (unsigned int i=0;igetCollisionShape()) { bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); - short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter; - short collisionFilterMask = isDynamic? btBroadphaseProxy::AllFilter : btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; + short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); + short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); addCollisionObject(body,collisionFilterGroup,collisionFilterMask); } } -void btDiscreteDynamicsWorld::updateVehicles(float timeStep) +void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) { BEGIN_PROFILE("updateVehicles"); - for (unsigned int i=0;iupdateVehicle( timeStep); @@ -338,11 +338,11 @@ void btDiscreteDynamicsWorld::updateVehicles(float timeStep) END_PROFILE("updateVehicles"); } -void btDiscreteDynamicsWorld::updateActivationState(float timeStep) +void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { BEGIN_PROFILE("updateActivationState"); - for (unsigned int i=0;i::iterator cit = std::find(m_constraints.begin(),m_constraints.end(),constraint); - if (!(cit==m_constraints.end())) - { - m_constraints.erase(cit); - } + m_constraints.remove(constraint); } void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) @@ -391,11 +387,7 @@ void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) { - std::vector::iterator vit = std::find(m_vehicles.begin(),m_vehicles.end(),vehicle); - if (!(vit==m_vehicles.end())) - { - m_vehicles.erase(vit); - } + m_vehicles.remove(vehicle); } inline int btGetConstraintIslandId(const btTypedConstraint* lhs) @@ -409,19 +401,26 @@ inline int btGetConstraintIslandId(const btTypedConstraint* lhs) } -static bool btSortConstraintOnIslandPredicate(const btTypedConstraint* lhs, const btTypedConstraint* rhs) + +class btSortConstraintOnIslandPredicate { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId(rhs); - lIslandId0 = btGetConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; -} + public: + + bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId(rhs); + lIslandId0 = btGetConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + + + void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { - BEGIN_PROFILE("solveConstraints"); - struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback { @@ -430,7 +429,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btTypedConstraint** m_sortedConstraints; int m_numConstraints; btIDebugDraw* m_debugDrawer; - + btStackAlloc* m_stackAlloc; InplaceSolverIslandCallback( @@ -438,17 +437,25 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btConstraintSolver* solver, btTypedConstraint** sortedConstraints, int numConstraints, - btIDebugDraw* debugDrawer) + btIDebugDraw* debugDrawer, + btStackAlloc* stackAlloc) :m_solverInfo(solverInfo), m_solver(solver), m_sortedConstraints(sortedConstraints), m_numConstraints(numConstraints), - m_debugDrawer(debugDrawer) + m_debugDrawer(debugDrawer), + m_stackAlloc(stackAlloc) { } - virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds, int islandId) + InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) { //also add all non-contact constraints/joints for this island btTypedConstraint** startConstraint = 0; @@ -473,36 +480,35 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } } - m_solver->solveGroup( manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer); + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc); } }; - - - - //sorted version of all btTypedConstraint, based on islandId - std::vector sortedConstraints; + btAlignedObjectArray sortedConstraints; sortedConstraints.resize( m_constraints.size()); int i; for (i=0;ibuildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); - END_PROFILE("solveConstraints"); } @@ -545,37 +551,6 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() } -static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color) -{ - - btVector3 halfExtents = (to-from)* 0.5f; - btVector3 center = (to+from) *0.5f; - int i,j; - - btVector3 edgecoord(1.f,1.f,1.f),pa,pb; - for (i=0;i<4;i++) - { - for (j=0;j<3;j++) - { - pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], - edgecoord[2]*halfExtents[2]); - pa+=center; - - int othercoord = j%3; - edgecoord[othercoord]*=-1.f; - pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], - edgecoord[2]*halfExtents[2]); - pb+=center; - - debugDrawer->drawLine(pa,pb,color); - } - edgecoord = btVector3(-1.f,-1.f,-1.f); - if (i<3) - edgecoord[i]*=-1.f; - } - - -} void btDiscreteDynamicsWorld::updateAabbs() { @@ -583,7 +558,7 @@ void btDiscreteDynamicsWorld::updateAabbs() btVector3 colorvec(1,0,0); btTransform predictedTrans; - for (unsigned int i=0;igetCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache; + btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < 1e12f)) + if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) { 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); static bool reportMe = true; - if (reportMe) + if (reportMe && m_debugDrawer) { reportMe = false; - printf("Overflow in AABB, object removed from simulation \n"); - printf("If you can reproduce this, please email bugs@continuousphysics.com\n"); - printf("Please include above information, your Platform, version of OS.\n"); - printf("Thanks.\n"); + m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); + m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); + m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); + m_debugDrawer->reportErrorWarning("Thanks.\n"); } } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { - DrawAabb(m_debugDrawer,minAabb,maxAabb,colorvec); + m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); } } } @@ -630,11 +604,11 @@ void btDiscreteDynamicsWorld::updateAabbs() END_PROFILE("updateAabbs"); } -void btDiscreteDynamicsWorld::integrateTransforms(float timeStep) +void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { BEGIN_PROFILE("integrateTransforms"); btTransform predictedTrans; - for (unsigned int i=0;i(shape); - float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin + btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin btVector3 start = worldTransform.getOrigin(); getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(radius,0,0),color); getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,radius,0),color); @@ -783,13 +761,13 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, case CONE_SHAPE_PROXYTYPE: { const btConeShape* coneShape = static_cast(shape); - float radius = coneShape->getRadius();//+coneShape->getMargin(); - float height = coneShape->getHeight();//+coneShape->getMargin(); + btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); + btScalar height = coneShape->getHeight();//+coneShape->getMargin(); btVector3 start = worldTransform.getOrigin(); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(radius,0.f,-0.5f*height),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(-radius,0.f,-0.5f*height),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(0.f,radius,-0.5f*height),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(0.f,-radius,-0.5f*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(radius,btScalar(0.),btScalar(-0.5)*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(-radius,btScalar(0.),btScalar(-0.5)*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),radius,btScalar(-0.5)*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),-radius,btScalar(-0.5)*height),color); break; } @@ -797,8 +775,8 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, { const btCylinderShape* cylinder = static_cast(shape); int upAxis = cylinder->getUpAxis(); - float radius = cylinder->getRadius(); - float halfHeight = cylinder->getHalfExtents()[upAxis]; + btScalar radius = cylinder->getRadius(); + btScalar halfHeight = cylinder->getHalfExtents()[upAxis]; btVector3 start = worldTransform.getOrigin(); btVector3 offsetHeight(0,0,0); offsetHeight[upAxis] = halfHeight; @@ -814,12 +792,12 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btTriangleMeshShape* concaveMesh = (btTriangleMeshShape*) shape; - //btVector3 aabbMax(1e30f,1e30f,1e30f); - //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); + //btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + //btVector3 aabbMax(100,100,100);//btScalar(1e30),btScalar(1e30),btScalar(1e30)); //todo pass camera, for some culling - btVector3 aabbMax(1e30f,1e30f,1e30f); - btVector3 aabbMin(-1e30f,-1e30f,-1e30f); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); @@ -830,8 +808,8 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, { btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; //todo: pass camera for some culling - btVector3 aabbMax(1e30f,1e30f,1e30f); - btVector3 aabbMin(-1e30f,-1e30f,-1e30f); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); //DebugDrawcallback drawCallback; DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 8575f8506f1..7ffa2c07ee6 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -23,12 +23,12 @@ class btOverlappingPairCache; class btConstraintSolver; class btSimulationIslandManager; class btTypedConstraint; -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "../ConstraintSolver/btContactSolverInfo.h" class btRaycastVehicle; class btIDebugDraw; +#include "../../LinearMath/btAlignedObjectArray.h" -#include ///btDiscreteDynamicsWorld provides discrete rigid body simulation ///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController @@ -40,14 +40,14 @@ protected: btSimulationIslandManager* m_islandManager; - std::vector m_constraints; + btAlignedObjectArray m_constraints; btIDebugDraw* m_debugDrawer; btVector3 m_gravity; //for variable timesteps - float m_localTime; + btScalar m_localTime; //for variable timesteps bool m_ownsIslandManager; @@ -56,29 +56,29 @@ protected: btContactSolverInfo m_solverInfo; - std::vector m_vehicles; + btAlignedObjectArray m_vehicles; int m_profileTimings; - void predictUnconstraintMotion(float timeStep); + void predictUnconstraintMotion(btScalar timeStep); - void integrateTransforms(float timeStep); + void integrateTransforms(btScalar timeStep); void calculateSimulationIslands(); void solveConstraints(btContactSolverInfo& solverInfo); - void updateActivationState(float timeStep); + void updateActivationState(btScalar timeStep); - void updateVehicles(float timeStep); + void updateVehicles(btScalar timeStep); - void startProfiling(float timeStep); + void startProfiling(btScalar timeStep); - virtual void internalSingleStepSimulation( float timeStep); + virtual void internalSingleStepSimulation( btScalar timeStep); void synchronizeMotionStates(); - void saveKinematicState(float timeStep); + void saveKinematicState(btScalar timeStep); public: @@ -90,7 +90,7 @@ public: virtual ~btDiscreteDynamicsWorld(); ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's - virtual int stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f); + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); virtual void updateAabbs(); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 617ed98d2af..d3ea7db8e5c 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef BT_DYNAMICS_WORLD_H #define BT_DYNAMICS_WORLD_H -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "../../BulletCollision/CollisionDispatch/btCollisionWorld.h" class btTypedConstraint; class btRaycastVehicle; class btConstraintSolver; @@ -39,17 +39,17 @@ class btDynamicsWorld : public btCollisionWorld ///stepSimulation proceeds the simulation over timeStep units ///if maxSubSteps > 0, it will interpolate time steps - virtual int stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f)=0; + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; virtual void updateAabbs() = 0; - virtual void addConstraint(btTypedConstraint* constraint) {}; + virtual void addConstraint(btTypedConstraint* constraint) { (void)constraint;}; - virtual void removeConstraint(btTypedConstraint* constraint) {}; + virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}; - virtual void addVehicle(btRaycastVehicle* vehicle) {}; + virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}; - virtual void removeVehicle(btRaycastVehicle* vehicle) {}; + virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}; virtual void setDebugDrawer(btIDebugDraw* debugDrawer) = 0; @@ -68,9 +68,9 @@ class btDynamicsWorld : public btCollisionWorld virtual int getNumConstraints() const { return 0; } - virtual btTypedConstraint* getConstraint(int index) { return 0; } + virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } - virtual const btTypedConstraint* getConstraint(int index) const { return 0; } + virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index 705c023d3a0..dbf73b85bcf 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -19,25 +19,25 @@ subject to the following restrictions: #include #include -float gLinearAirDamping = 1.f; +btScalar gLinearAirDamping = btScalar(1.); //'temporarily' global variables -float gDeactivationTime = 2.f; +btScalar gDeactivationTime = btScalar(2.); bool gDisableDeactivation = false; -float gLinearSleepingThreshold = 0.8f; -float gAngularSleepingThreshold = 1.0f; +btScalar gLinearSleepingThreshold = btScalar(0.8); +btScalar gAngularSleepingThreshold = btScalar(1.0); static int uniqueId = 0; -btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) +btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) : - m_gravity(0.0f, 0.0f, 0.0f), - m_totalForce(0.0f, 0.0f, 0.0f), - m_totalTorque(0.0f, 0.0f, 0.0f), - m_linearVelocity(0.0f, 0.0f, 0.0f), - m_angularVelocity(0.f,0.f,0.f), - m_angularFactor(1.f), - m_linearDamping(0.f), - m_angularDamping(0.5f), + m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)), + m_angularFactor(btScalar(1.)), + m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_linearDamping(btScalar(0.)), + m_angularDamping(btScalar(0.5)), m_optionalMotionState(motionState), m_contactSolverType(0), m_frictionSolverType(0) @@ -72,15 +72,15 @@ 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) +btRigidBody::btRigidBody( btScalar 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), - m_totalForce(0.0f, 0.0f, 0.0f), - m_totalTorque(0.0f, 0.0f, 0.0f), - m_linearVelocity(0.0f, 0.0f, 0.0f), - m_angularVelocity(0.f,0.f,0.f), - m_linearDamping(0.f), - m_angularDamping(0.5f), + m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), + m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)), + m_linearDamping(btScalar(0.)), + m_angularDamping(btScalar(0.5)), m_optionalMotionState(0), m_contactSolverType(0), m_frictionSolverType(0) @@ -110,16 +110,18 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi #endif //OBSOLETE_MOTIONSTATE_LESS -//#define EXPERIMENTAL_JITTER_REMOVAL 1 + + +#define EXPERIMENTAL_JITTER_REMOVAL 1 #ifdef EXPERIMENTAL_JITTER_REMOVAL //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate //doesn't work very well yet (value 0 disabled this damping) //note there this influences deactivation thresholds! -float gClippedAngvelThresholdSqr = 0.01f; -float gClippedLinearThresholdSqr = 0.01f; +btScalar gClippedAngvelThresholdSqr = btScalar(0.01); +btScalar gClippedLinearThresholdSqr = btScalar(0.01); #endif //EXPERIMENTAL_JITTER_REMOVAL -float gJitterVelocityDampingFactor = 1.f; +btScalar gJitterVelocityDampingFactor = btScalar(0.7); void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) { @@ -144,7 +146,7 @@ void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& pred void btRigidBody::saveKinematicState(btScalar timeStep) { //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities - if (timeStep != 0.f) + if (timeStep != btScalar(0.)) { //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform if (getMotionState()) @@ -169,9 +171,9 @@ void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const void btRigidBody::setGravity(const btVector3& acceleration) { - if (m_inverseMass != 0.0f) + if (m_inverseMass != btScalar(0.0)) { - m_gravity = acceleration * (1.0f / m_inverseMass); + m_gravity = acceleration * (btScalar(1.0) / m_inverseMass); } } @@ -182,8 +184,8 @@ void btRigidBody::setGravity(const btVector3& acceleration) void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) { - m_linearDamping = GEN_clamped(lin_damping, 0.0f, 1.0f); - m_angularDamping = GEN_clamped(ang_damping, 0.0f, 1.0f); + m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); + m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } @@ -198,36 +200,36 @@ void btRigidBody::applyForces(btScalar step) applyCentralForce(m_gravity); - m_linearVelocity *= GEN_clamped((1.f - step * gLinearAirDamping * m_linearDamping), 0.0f, 1.0f); - m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f); + m_linearVelocity *= GEN_clamped((btScalar(1.) - step * gLinearAirDamping * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); + m_angularVelocity *= GEN_clamped((btScalar(1.) - step * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); #define FORCE_VELOCITY_DAMPING 1 #ifdef FORCE_VELOCITY_DAMPING - float speed = m_linearVelocity.length(); + btScalar speed = m_linearVelocity.length(); if (speed < m_linearDamping) { - float dampVel = 0.005f; + btScalar dampVel = btScalar(0.005); if (speed > dampVel) { btVector3 dir = m_linearVelocity.normalized(); m_linearVelocity -= dir * dampVel; } else { - m_linearVelocity.setValue(0.f,0.f,0.f); + m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } } - float angSpeed = m_angularVelocity.length(); + btScalar angSpeed = m_angularVelocity.length(); if (angSpeed < m_angularDamping) { - float angDampVel = 0.005f; + btScalar angDampVel = btScalar(0.005); if (angSpeed > angDampVel) { btVector3 dir = m_angularVelocity.normalized(); m_angularVelocity -= dir * angDampVel; } else { - m_angularVelocity.setValue(0.f,0.f,0.f); + m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } } #endif //FORCE_VELOCITY_DAMPING @@ -242,19 +244,19 @@ void btRigidBody::proceedToTransform(const btTransform& newTrans) void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { - if (mass == 0.f) + if (mass == btScalar(0.)) { m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; - m_inverseMass = 0.f; + m_inverseMass = btScalar(0.); } else { m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); - m_inverseMass = 1.0f / mass; + m_inverseMass = btScalar(1.0) / mass; } - m_invInertiaLocal.setValue(inertia[0] != 0.0f ? 1.0f / inertia[0]: 0.0f, - inertia[1] != 0.0f ? 1.0f / inertia[1]: 0.0f, - inertia[2] != 0.0f ? 1.0f / inertia[2]: 0.0f); + m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0), + inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0), + inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0)); } @@ -276,7 +278,7 @@ void btRigidBody::integrateVelocities(btScalar step) #define MAX_ANGVEL SIMD_HALF_PI /// clamp angular velocity. collision calculations will fail on higher angular velocities - float angvel = m_angularVelocity.length(); + btScalar angvel = m_angularVelocity.length(); if (angvel*step > MAX_ANGVEL) { m_angularVelocity *= (MAX_ANGVEL/step) /angvel; @@ -295,7 +297,7 @@ btQuaternion btRigidBody::getOrientation() const void btRigidBody::setCenterOfMassTransform(const btTransform& xform) { - m_interpolationWorldTransform = m_worldTransform; + m_interpolationWorldTransform = xform;//m_worldTransform; m_interpolationLinearVelocity = getLinearVelocity(); m_interpolationAngularVelocity = getAngularVelocity(); m_worldTransform = xform; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index 43869363cdf..9da545adb68 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -16,26 +16,22 @@ subject to the following restrictions: #ifndef RIGIDBODY_H #define RIGIDBODY_H -#include -#include -#include -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" - - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "../../LinearMath/btPoint3.h" +#include "../../LinearMath/btTransform.h" +#include "../../BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "../../BulletCollision/CollisionDispatch/btCollisionObject.h" class btCollisionShape; class btMotionState; -extern float gLinearAirDamping; -extern bool gUseEpa; +extern btScalar gLinearAirDamping; -extern float gDeactivationTime; +extern btScalar gDeactivationTime; extern bool gDisableDeactivation; -extern float gLinearSleepingThreshold; -extern float gAngularSleepingThreshold; +extern btScalar gLinearSleepingThreshold; +extern btScalar gAngularSleepingThreshold; /// btRigidBody class for btRigidBody Dynamics @@ -65,10 +61,10 @@ 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); + btRigidBody(btScalar mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.)); #endif //OBSOLETE_MOTIONSTATE_LESS - btRigidBody(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); + btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.)); void proceedToTransform(const btTransform& newTrans); @@ -157,7 +153,7 @@ public: void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) { - if (m_inverseMass != 0.f) + if (m_inverseMass != btScalar(0.)) { applyCentralImpulse(impulse); if (m_angularFactor) @@ -168,9 +164,9 @@ public: } //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,float impulseMagnitude) + inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) { - if (m_inverseMass != 0.f) + if (m_inverseMass != btScalar(0.)) { m_linearVelocity += linearComponent*impulseMagnitude; if (m_angularFactor) @@ -182,8 +178,8 @@ public: void clearForces() { - m_totalForce.setValue(0.0f, 0.0f, 0.0f); - m_totalTorque.setValue(0.0f, 0.0f, 0.0f); + m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); } void updateInertiaTensor(); @@ -238,7 +234,7 @@ public: - inline float computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const + inline btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const { btVector3 r0 = pos - getCenterOfMassPosition(); @@ -250,13 +246,13 @@ public: } - inline float computeAngularImpulseDenominator(const btVector3& axis) const + inline btScalar computeAngularImpulseDenominator(const btVector3& axis) const { btVector3 vec = axis * getInvInertiaTensorWorld(); return axis.dot(vec); } - inline void updateDeactivation(float timeStep) + inline void updateDeactivation(btScalar timeStep) { if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) return; @@ -267,7 +263,7 @@ public: m_deactivationTime += timeStep; } else { - m_deactivationTime=0.f; + m_deactivationTime=btScalar(0.); setActivationState(0); } @@ -280,7 +276,7 @@ public: return false; //disable deactivation - if (gDisableDeactivation || (gDeactivationTime == 0.f)) + if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) return false; if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) @@ -328,11 +324,11 @@ public: int m_contactSolverType; int m_frictionSolverType; - void setAngularFactor(float angFac) + void setAngularFactor(btScalar angFac) { m_angularFactor = angFac; } - float getAngularFactor() const + btScalar getAngularFactor() const { return m_angularFactor; } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index fe0124c041a..4ebcb8e7517 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -22,6 +22,14 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +/* + Make sure this dummy function never changes so that it + can be used by probes that are checking whether the + library is actually installed. +*/ +extern "C" void btBulletDynamicsProbe () {} + + btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) @@ -41,8 +49,12 @@ btSimpleDynamicsWorld::~btSimpleDynamicsWorld() delete m_constraintSolver; } -int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, float fixedTimeStep) +int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) { + (void)fixedTimeStep; + (void)maxSubSteps; + + ///apply gravity, predict motion predictUnconstraintMotion(timeStep); @@ -63,7 +75,7 @@ int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; - m_constraintSolver->solveGroup(manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer); + m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc); } ///integrate transforms @@ -81,7 +93,7 @@ int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for (unsigned int i=0;i= -0.1f) + if ( denominator >= btScalar(-0.1)) { - wheel.m_suspensionRelativeVelocity = 0.0f; - wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f; + wheel.m_suspensionRelativeVelocity = btScalar(0.0); + wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); } else { - btScalar inv = -1.f / denominator; + btScalar inv = btScalar(-1.) / denominator; wheel.m_suspensionRelativeVelocity = projVel * inv; wheel.m_clippedInvContactDotSuspension = inv; } @@ -233,9 +235,9 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) { //put wheel info as in rest position wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); - wheel.m_suspensionRelativeVelocity = 0.0f; + wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; - wheel.m_clippedInvContactDotSuspension = 1.0f; + wheel.m_clippedInvContactDotSuspension = btScalar(1.0); } return depth; @@ -267,7 +269,7 @@ void btRaycastVehicle::updateVehicle( btScalar step ) } - m_currentVehicleSpeedKmHour = 3.6f * getRigidBody()->getLinearVelocity().length(); + m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length(); const btTransform& chassisTrans = getChassisWorldTransform(); @@ -276,9 +278,9 @@ void btRaycastVehicle::updateVehicle( btScalar step ) chassisTrans.getBasis()[1][m_indexForwardAxis], chassisTrans.getBasis()[2][m_indexForwardAxis]); - if (forwardW.dot(getRigidBody()->getLinearVelocity()) < 0.f) + if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.)) { - m_currentVehicleSpeedKmHour *= -1.f; + m_currentVehicleSpeedKmHour *= btScalar(-1.); } // @@ -300,9 +302,9 @@ void btRaycastVehicle::updateVehicle( btScalar step ) //apply suspension force btWheelInfo& wheel = m_wheelInfo[i]; - float suspensionForce = wheel.m_wheelsSuspensionForce; + btScalar suspensionForce = wheel.m_wheelsSuspensionForce; - float gMaxSuspensionForce = 6000.f; + btScalar gMaxSuspensionForce = btScalar(6000.); if (suspensionForce > gMaxSuspensionForce) { suspensionForce = gMaxSuspensionForce; @@ -347,7 +349,7 @@ void btRaycastVehicle::updateVehicle( btScalar step ) wheel.m_rotation += wheel.m_deltaRotation; } - wheel.m_deltaRotation *= 0.99f;//damping of rotation when not in contact + wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact } @@ -394,17 +396,18 @@ btWheelInfo& btRaycastVehicle::getWheelInfo(int index) return m_wheelInfo[index]; } -void btRaycastVehicle::setBrake(float brake,int wheelIndex) +void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex) { btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels())); - getWheelInfo(wheelIndex).m_brake; + getWheelInfo(wheelIndex).m_brake = brake; } void btRaycastVehicle::updateSuspension(btScalar deltaTime) { + (void)deltaTime; - btScalar chassisMass = 1.f / m_chassisBody->getInvMass(); + btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass(); for (int w_it=0; w_itcomputeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); + btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); + btScalar relaxation = 1.f; + m_jacDiagABInv = relaxation/(denom0+denom1); + } + + + +}; + +btScalar calcRollingFriction(btWheelContactPoint& contactPoint) +{ + + btScalar j1=0.f; + + const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld; + + btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); + btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition(); + + btScalar maxImpulse = contactPoint.m_maxImpulse; + + btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel); + + // calculate j that moves us to zero relative velocity + j1 = -vrel * contactPoint.m_jacDiagABInv; + GEN_set_min(j1, maxImpulse); + GEN_set_max(j1, -maxImpulse); + + return j1; +} + + + + +btScalar sideFrictionStiffness2 = btScalar(1.0); void btRaycastVehicle::updateFriction(btScalar timeStep) { @@ -481,8 +541,8 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; if (groundObject) numWheelsOnGround++; - sideImpulse[i] = 0.f; - forwardImpulse[i] = 0.f; + sideImpulse[i] = btScalar(0.); + forwardImpulse[i] = btScalar(0.); } @@ -517,7 +577,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS, *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, - 0.f, axle[i],sideImpulse[i],timeStep); + btScalar(0.), axle[i],sideImpulse[i],timeStep); sideImpulse[i] *= sideFrictionStiffness2; @@ -527,7 +587,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) } } - btScalar sideFactor = 1.f; + btScalar sideFactor = btScalar(1.); btScalar fwdFactor = 0.5; bool sliding = false; @@ -537,25 +597,46 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) btWheelInfo& wheelInfo = m_wheelInfo[wheel]; class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; + btScalar rollingFriction = 0.f; - forwardImpulse[wheel] = 0.f; - m_wheelInfo[wheel].m_skidInfo= 1.f; + if (groundObject) + { + if (wheelInfo.m_engineForce != 0.f) + { + rollingFriction = wheelInfo.m_engineForce* timeStep; + } else + { + btScalar defaultRollingFrictionImpulse = 0.f; + btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; + btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,forwardWS[wheel],maxImpulse); + rollingFriction = calcRollingFriction(contactPt); + } + } + + //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) + + + + + forwardImpulse[wheel] = btScalar(0.); + m_wheelInfo[wheel].m_skidInfo= btScalar(1.); if (groundObject) { - m_wheelInfo[wheel].m_skidInfo= 1.f; + m_wheelInfo[wheel].m_skidInfo= btScalar(1.); btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip; btScalar maximpSide = maximp; btScalar maximpSquared = maximp * maximpSide; + - forwardImpulse[wheel] = wheelInfo.m_engineForce* timeStep; + forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep; - float x = (forwardImpulse[wheel] ) * fwdFactor; - float y = (sideImpulse[wheel] ) * sideFactor; + btScalar x = (forwardImpulse[wheel] ) * fwdFactor; + btScalar y = (sideImpulse[wheel] ) * sideFactor; - float impulseSquared = (x*x + y*y); + btScalar impulseSquared = (x*x + y*y); if (impulseSquared > maximpSquared) { @@ -577,9 +658,9 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) { for (int wheel = 0;wheel < getNumWheels(); wheel++) { - if (sideImpulse[wheel] != 0.f) + if (sideImpulse[wheel] != btScalar(0.)) { - if (m_wheelInfo[wheel].m_skidInfo< 1.f) + if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.)) { forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; @@ -597,11 +678,11 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - m_chassisBody->getCenterOfMassPosition(); - if (forwardImpulse[wheel] != 0.f) + if (forwardImpulse[wheel] != btScalar(0.)) { m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos); } - if (sideImpulse[wheel] != 0.f) + if (sideImpulse[wheel] != btScalar(0.)) { class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h index fa961e468d7..f4249599615 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -11,11 +11,11 @@ #ifndef RAYCASTVEHICLE_H #define RAYCASTVEHICLE_H -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "../Dynamics/btRigidBody.h" +#include "../ConstraintSolver/btTypedConstraint.h" #include "btVehicleRaycaster.h" class btDynamicsWorld; -#include "LinearMath/btAlignedObjectArray.h" +#include "../../LinearMath/btAlignedObjectArray.h" #include "btWheelInfo.h" class btVehicleTuning; @@ -29,18 +29,18 @@ public: public: btVehicleTuning() - :m_suspensionStiffness(5.88f), - m_suspensionCompression(0.83f), - m_suspensionDamping(0.88f), - m_maxSuspensionTravelCm(500.f), - m_frictionSlip(10.5f) + :m_suspensionStiffness(btScalar(5.88)), + m_suspensionCompression(btScalar(0.83)), + m_suspensionDamping(btScalar(0.88)), + m_maxSuspensionTravelCm(btScalar(500.)), + m_frictionSlip(btScalar(10.5)) { } - float m_suspensionStiffness; - float m_suspensionCompression; - float m_suspensionDamping; - float m_maxSuspensionTravelCm; - float m_frictionSlip; + btScalar m_suspensionStiffness; + btScalar m_suspensionCompression; + btScalar m_suspensionDamping; + btScalar m_maxSuspensionTravelCm; + btScalar m_frictionSlip; }; private: @@ -48,9 +48,9 @@ private: btScalar m_tau; btScalar m_damping; btVehicleRaycaster* m_vehicleRaycaster; - float m_pitchControl; - float m_steeringValue; - float m_currentVehicleSpeedKmHour; + btScalar m_pitchControl; + btScalar m_steeringValue; + btScalar m_currentVehicleSpeedKmHour; btRigidBody* m_chassisBody; @@ -105,9 +105,9 @@ public: void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true); - void setBrake(float brake,int wheelIndex); + void setBrake(btScalar brake,int wheelIndex); - void setPitchControl(float pitch) + void setPitchControl(btScalar pitch) { m_pitchControl = pitch; } @@ -142,6 +142,26 @@ public: return m_indexForwardAxis; } + + ///Worldspace forward vector + btVector3 getForwardVector() const + { + const btTransform& chassisTrans = getChassisWorldTransform(); + + btVector3 forwardW ( + chassisTrans.getBasis()[0][m_indexForwardAxis], + chassisTrans.getBasis()[1][m_indexForwardAxis], + chassisTrans.getBasis()[2][m_indexForwardAxis]); + + return forwardW; + } + + ///Velocity of vehicle (positive if velocity vector has same direction as foward vector) + btScalar getCurrentSpeedKmHour() const + { + return m_currentVehicleSpeedKmHour; + } + virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex) { m_indexRightAxis = rightIndex; @@ -156,6 +176,7 @@ public: virtual void solveConstraint(btScalar timeStep) { + (void)timeStep; //not yet } diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h index 5be3693fe73..64a47fcaada 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h @@ -11,7 +11,7 @@ #ifndef VEHICLE_RAYCASTER_H #define VEHICLE_RAYCASTER_H -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" /// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting struct btVehicleRaycaster @@ -21,7 +21,7 @@ virtual ~btVehicleRaycaster() } struct btVehicleRaycasterResult { - btVehicleRaycasterResult() :m_distFraction(-1.f){}; + btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){}; btVector3 m_hitPointInWorld; btVector3 m_hitNormalInWorld; btScalar m_distFraction; diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp index 43baf56edbc..ef93c16fffc 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp @@ -21,6 +21,7 @@ btScalar btWheelInfo::getSuspensionRestLength() const void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo) { + (void)raycastInfo; if (m_raycastInfo.m_isInContact) @@ -31,14 +32,14 @@ void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInf btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); - if ( project >= -0.1f) + if ( project >= btScalar(-0.1)) { - m_suspensionRelativeVelocity = 0.0f; - m_clippedInvContactDotSuspension = 1.0f / 0.1f; + m_suspensionRelativeVelocity = btScalar(0.0); + m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); } else { - btScalar inv = -1.f / project; + btScalar inv = btScalar(-1.) / project; m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; } @@ -48,8 +49,8 @@ void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInf else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); - m_suspensionRelativeVelocity = 0.0f; + m_suspensionRelativeVelocity = btScalar(0.0); m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; - m_clippedInvContactDotSuspension = 1.0f; + m_clippedInvContactDotSuspension = btScalar(1.0); } } diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h index 83a3b114ab9..2e349b3fde4 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h @@ -11,8 +11,8 @@ #ifndef WHEEL_INFO_H #define WHEEL_INFO_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" class btRigidBody; @@ -25,10 +25,10 @@ struct btWheelInfoConstructionInfo btScalar m_maxSuspensionTravelCm; btScalar m_wheelRadius; - float m_suspensionStiffness; - float m_wheelsDampingCompression; - float m_wheelsDampingRelaxation; - float m_frictionSlip; + btScalar m_suspensionStiffness; + btScalar m_wheelsDampingCompression; + btScalar m_wheelsDampingRelaxation; + btScalar m_frictionSlip; bool m_bIsFrontWheel; }; @@ -92,12 +92,12 @@ struct btWheelInfo m_wheelDirectionCS = ci.m_wheelDirectionCS; m_wheelAxleCS = ci.m_wheelAxleCS; m_frictionSlip = ci.m_frictionSlip; - m_steering = 0.f; - m_engineForce = 0.f; - m_rotation = 0.f; - m_deltaRotation = 0.f; - m_brake = 0.f; - m_rollInfluence = 0.1f; + m_steering = btScalar(0.); + m_engineForce = btScalar(0.); + m_rotation = btScalar(0.); + m_deltaRotation = btScalar(0.); + m_brake = btScalar(0.); + m_rollInfluence = btScalar(0.1); m_bIsFrontWheel = ci.m_bIsFrontWheel; } diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet2/src/LinearMath/btAabbUtil2.h index 2eacb8e242b..429163c8138 100644 --- a/extern/bullet2/src/LinearMath/btAabbUtil2.h +++ b/extern/bullet2/src/LinearMath/btAabbUtil2.h @@ -17,8 +17,8 @@ subject to the following restrictions: #ifndef AABB_UTIL2 #define AABB_UTIL2 -#include "LinearMath/btVector3.h" -#include "LinearMath/btSimdMinMax.h" +#include "btVector3.h" +#include "btSimdMinMax.h" #define btMin(a,b) ((a < b ? a : b)) @@ -73,8 +73,8 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, const btVector3& aabbMax, btScalar& param, btVector3& normal) { - btVector3 aabbHalfExtent = (aabbMax-aabbMin)* 0.5f; - btVector3 aabbCenter = (aabbMax+aabbMin)* 0.5f; + btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5); + btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5); btVector3 source = rayFrom - aabbCenter; btVector3 target = rayTo - aabbCenter; int sourceOutcode = btOutcode(source,aabbHalfExtent); @@ -110,7 +110,7 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, } bit<<=1; } - normSign = -1.f; + normSign = btScalar(-1.); } if (lambda_enter <= lambda_exit) { diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp index 19b811b0ce8..1f5877fa37e 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btAlignedAllocator.h" -#if defined (WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__) +#if defined (BT_HAS_ALIGNED_ALOCATOR) #include void* btAlignedAlloc (int size, int alignment) @@ -31,6 +31,26 @@ void btAlignedFree (void* ptr) #else +#ifdef __CELLOS_LV2__ + +#include + +int numAllocs = 0; +int numFree = 0; + +void* btAlignedAlloc (int size, int alignment) +{ + numAllocs++; + return memalign(alignment, size); +} + +void btAlignedFree (void* ptr) +{ + numFree++; + free(ptr); +} + +#else ///todo ///will add some multi-platform version that works without _aligned_malloc/_aligned_free @@ -41,7 +61,10 @@ void* btAlignedAlloc (int size, int alignment) void btAlignedFree (void* ptr) { - delete ptr; + delete [] (char*) ptr; } +#endif // #endif + + diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.h b/extern/bullet2/src/LinearMath/btAlignedAllocator.h index 159399ce7d4..07585717f45 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.h +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.h @@ -36,11 +36,13 @@ class btAlignedAllocator { typedef btAlignedAllocator< T , Alignment > self_type; public: + //just going down a list: btAlignedAllocator() {} - + /* btAlignedAllocator( const self_type & ) {} - + */ + template < typename Other > btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {} @@ -53,6 +55,7 @@ public: pointer address ( reference ref ) const { return &ref; } const_pointer address ( const_reference ref ) const { return &ref; } pointer allocate ( size_type n , const_pointer * hint = 0 ) { + (void)hint; return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment )); } void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); } diff --git a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h index 3bfdb36f4c7..8bef5eb5d06 100644 --- a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h +++ b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h @@ -20,18 +20,37 @@ subject to the following restrictions: #include "btScalar.h" // has definitions like SIMD_FORCE_INLINE #include "btAlignedAllocator.h" +///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW +///then the btAlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors +///You can enable BT_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator= +///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and +///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240 + +#define BT_USE_PLACEMENT_NEW 1 +//#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in or or otherwise... + +#ifdef BT_USE_MEMCPY +#include +#include +#endif //BT_USE_MEMCPY + +#ifdef BT_USE_PLACEMENT_NEW +#include //for placement new +#endif //BT_USE_PLACEMENT_NEW + + ///btAlignedObjectArray uses a subset of the stl::vector interface for its methods ///It is developed to replace stl::vector to avoid STL alignment issues to add SIMD/SSE data template //template class btAlignedObjectArray { + btAlignedAllocator m_allocator; + int m_size; int m_capacity; T* m_data; - btAlignedAllocator m_allocator; - protected: SIMD_FORCE_INLINE int allocSize(int size) { @@ -40,8 +59,12 @@ class btAlignedObjectArray SIMD_FORCE_INLINE void copy(int start,int end, T* dest) { int i; - for (i=0;i size()) + int curSize = size(); + + if (newsize < size()) + { + for(int i = curSize; i < newsize; i++) + { + m_data[i].~T(); + } + } else { - reserve(newsize); + if (newsize > size()) + { + reserve(newsize); + } +#ifdef BT_USE_PLACEMENT_NEW + for (int i=curSize;i + void downHeap(T *pArr, int k, int n,L CompareFunc) + { + /* PRE: a[k+1..N] is a heap */ + /* POST: a[k..N] is a heap */ + + T temp = pArr[k - 1]; + /* k has child(s) */ + while (k <= n/2) + { + int child = 2*k; + + if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child])) + { + child++; + } + /* pick larger child */ + if (CompareFunc(temp , pArr[child - 1])) + { + /* move child up */ + pArr[k - 1] = pArr[child - 1]; + k = child; } + else + { + break; + } + } + pArr[k - 1] = temp; + } /*downHeap*/ + + void swap(int index0,int index1) + { +#ifdef BT_USE_MEMCPY + char temp[sizeof(T)]; + memcpy(temp,&m_data[index0],sizeof(T)); + memcpy(&m_data[index0],&m_data[index1],sizeof(T)); + memcpy(&m_data[index1],temp,sizeof(T)); +#else + T temp = m_data[index0]; + m_data[index0] = m_data[index1]; + m_data[index1] = temp; +#endif //BT_USE_PLACEMENT_NEW + + } + + template + void heapSort(L CompareFunc) + { + /* sort a[0..N-1], N.B. 0 to N-1 */ + int k; + int n = m_size; + for (k = n/2; k > 0; k--) + { + downHeap(m_data, k, n, CompareFunc); + } + + /* a[1..N] is now a heap */ + while ( n>=1 ) + { + swap(0,n-1); /* largest of a[0..n-1] */ + + + n = n - 1; + /* restore a[1..i-1] heap */ + downHeap(m_data, 1, n, CompareFunc); + } + } + + ///non-recursive binary search, assumes sorted array + int findBinarySearch(const T& key) const + { + int first = 0; + int last = size(); + + //assume sorted array + while (first <= last) { + int mid = (first + last) / 2; // compute mid point. + if (key > m_data[mid]) + first = mid + 1; // repeat search in top half. + else if (key < m_data[mid]) + last = mid - 1; // repeat search in bottom half. + else + return mid; // found it. return position ///// + } + return size(); // failed to find key + } + + + int findLinearSearch(const T& key) const + { + int index=size(); + int i; + + for (i=0;i& planeEquations, const btVector3& point, float margin) + +/* + Make sure this dummy function never changes so that it + can be used by probes that are checking whether the + library is actually installed. +*/ +extern "C" void btBulletMathProbe () {} + + +bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin) { int numbrushes = planeEquations.size(); for (int i=0;i0.f) + btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin; + if (dist>btScalar(0.)) { return false; } @@ -33,14 +42,14 @@ bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray& } -bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, float margin) +bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin) { int numvertices = vertices.size(); for (int i=0;i0.f) + btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin; + if (dist>btScalar(0.)) { return false; } @@ -54,7 +63,7 @@ bool notExist(const btVector3& planeEquation,const btAlignedObjectArray 0.999f) + if (planeEquation.dot(N1) > btScalar(0.999)) { return false; } @@ -83,11 +92,11 @@ void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray 0.0001f) + if (planeEquation.length2() > btScalar(0.0001)) { planeEquation.normalize(); if (notExist(planeEquation,planeEquationsOut)) @@ -95,13 +104,13 @@ void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray 0.0001f ) && - ( n3n1.length2() > 0.0001f ) && - ( n1n2.length2() > 0.0001f ) ) + if ( ( n2n3.length2() > btScalar(0.0001) ) && + ( n3n1.length2() > btScalar(0.0001) ) && + ( n1n2.length2() > btScalar(0.0001) ) ) { //point P out of 3 plane equations: @@ -143,10 +152,10 @@ void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray 0.000001f) + btScalar quotient = (N1.dot(n2n3)); + if (btFabs(quotient) > btScalar(0.000001)) { - quotient = -1.f / quotient; + quotient = btScalar(-1.) / quotient; n2n3 *= N1[3]; n3n1 *= N2[3]; n1n2 *= N3[3]; @@ -156,7 +165,7 @@ void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations , btAlignedObjectArray& verticesOut ); - static bool isInside(const btAlignedObjectArray& vertices, const btVector3& planeNormal, float margin); + static bool isInside(const btAlignedObjectArray& vertices, const btVector3& planeNormal, btScalar margin); - static bool isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, float margin); + static bool isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin); - static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, float margin); + static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin); }; diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h index 86db735ce94..5f40ca39157 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -28,7 +28,7 @@ DEALINGS IN THE SOFTWARE. #ifndef IDEBUG_DRAW__H #define IDEBUG_DRAW__H -#include "LinearMath/btVector3.h" +#include "btVector3.h" class btIDebugDraw @@ -56,14 +56,45 @@ class btIDebugDraw virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0; - virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,float distance,int lifeTime,const btVector3& color)=0; + virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0; + + virtual void reportErrorWarning(const char* warningString) = 0; virtual void setDebugMode(int debugMode) =0; virtual int getDebugMode() const = 0; + inline void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color) + { + btVector3 halfExtents = (to-from)* 0.5f; + btVector3 center = (to+from) *0.5f; + int i,j; + + btVector3 edgecoord(1.f,1.f,1.f),pa,pb; + for (i=0;i<4;i++) + { + for (j=0;j<3;j++) + { + pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], + edgecoord[2]*halfExtents[2]); + pa+=center; + + int othercoord = j%3; + edgecoord[othercoord]*=-1.f; + pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], + edgecoord[2]*halfExtents[2]); + pb+=center; + + drawLine(pa,pb,color); + } + edgecoord = btVector3(-1.f,-1.f,-1.f); + if (i<3) + edgecoord[i]*=-1.f; + } + } }; + #endif //IDEBUG_DRAW__H diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h index c3cc90a82c7..fda348b42ab 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -16,10 +16,10 @@ subject to the following restrictions: #ifndef btMatrix3x3_H #define btMatrix3x3_H -#include "LinearMath/btScalar.h" +#include "btScalar.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btQuaternion.h" +#include "btVector3.h" +#include "btQuaternion.h" class btMatrix3x3 { @@ -45,12 +45,14 @@ class btMatrix3x3 { zx, zy, zz); } - btVector3 getColumn(int i) const + SIMD_FORCE_INLINE btVector3 getColumn(int i) const { return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); } + + - const btVector3& getRow(int i) const + SIMD_FORCE_INLINE const btVector3& getRow(int i) const { return m_el[i]; } @@ -58,13 +60,13 @@ class btMatrix3x3 { SIMD_FORCE_INLINE btVector3& operator[](int i) { - assert(0 <= i && i < 3); + btFullAssert(0 <= i && i < 3); return m_el[i]; } - const btVector3& operator[](int i) const + SIMD_FORCE_INLINE const btVector3& operator[](int i) const { - assert(0 <= i && i < 3); + btFullAssert(0 <= i && i < 3); return m_el[i]; } @@ -73,41 +75,30 @@ class btMatrix3x3 { void setFromOpenGLSubMatrix(const btScalar *m) { - m_el[0][0] = (m[0]); - m_el[1][0] = (m[1]); - m_el[2][0] = (m[2]); - m_el[0][1] = (m[4]); - m_el[1][1] = (m[5]); - m_el[2][1] = (m[6]); - m_el[0][2] = (m[8]); - m_el[1][2] = (m[9]); - m_el[2][2] = (m[10]); + m_el[0].setValue(m[0],m[4],m[8]); + m_el[1].setValue(m[1],m[5],m[9]); + m_el[2].setValue(m[2],m[6],m[10]); + } void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& zx, const btScalar& zy, const btScalar& zz) { - m_el[0][0] = btScalar(xx); - m_el[0][1] = btScalar(xy); - m_el[0][2] = btScalar(xz); - m_el[1][0] = btScalar(yx); - m_el[1][1] = btScalar(yy); - m_el[1][2] = btScalar(yz); - m_el[2][0] = btScalar(zx); - m_el[2][1] = btScalar(zy); - m_el[2][2] = btScalar(zz); + m_el[0].setValue(xx,xy,xz); + m_el[1].setValue(yx,yy,yz); + m_el[2].setValue(zx,zy,zz); } void setRotation(const btQuaternion& q) { btScalar d = q.length2(); - assert(d != btScalar(0.0)); + btFullAssert(d != btScalar(0.0)); btScalar s = btScalar(2.0) / d; - btScalar xs = q[0] * s, ys = q[1] * s, zs = q[2] * s; - btScalar wx = q[3] * xs, wy = q[3] * ys, wz = q[3] * zs; - btScalar xx = q[0] * xs, xy = q[0] * ys, xz = q[0] * zs; - btScalar yy = q[1] * ys, yz = q[1] * zs, zz = q[2] * zs; + btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; + btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; + btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; + btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy, xy + wz, btScalar(1.0) - (xx + zz), yz - wx, xz - wy, yz + wx, btScalar(1.0) - (xx + yy)); @@ -168,90 +159,88 @@ class btMatrix3x3 { void getOpenGLSubMatrix(btScalar *m) const { - m[0] = btScalar(m_el[0][0]); - m[1] = btScalar(m_el[1][0]); - m[2] = btScalar(m_el[2][0]); + m[0] = btScalar(m_el[0].x()); + m[1] = btScalar(m_el[1].x()); + m[2] = btScalar(m_el[2].x()); m[3] = btScalar(0.0); - m[4] = btScalar(m_el[0][1]); - m[5] = btScalar(m_el[1][1]); - m[6] = btScalar(m_el[2][1]); + m[4] = btScalar(m_el[0].y()); + m[5] = btScalar(m_el[1].y()); + m[6] = btScalar(m_el[2].y()); m[7] = btScalar(0.0); - m[8] = btScalar(m_el[0][2]); - m[9] = btScalar(m_el[1][2]); - m[10] = btScalar(m_el[2][2]); + m[8] = btScalar(m_el[0].z()); + m[9] = btScalar(m_el[1].z()); + m[10] = btScalar(m_el[2].z()); m[11] = btScalar(0.0); } void getRotation(btQuaternion& q) const { - btScalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2]; + btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); + btScalar temp[4]; if (trace > btScalar(0.0)) { btScalar s = btSqrt(trace + btScalar(1.0)); - q[3] = s * btScalar(0.5); + temp[3]=(s * btScalar(0.5)); s = btScalar(0.5) / s; - q[0] = (m_el[2][1] - m_el[1][2]) * s; - q[1] = (m_el[0][2] - m_el[2][0]) * s; - q[2] = (m_el[1][0] - m_el[0][1]) * s; + temp[0]=((m_el[2].y() - m_el[1].z()) * s); + temp[1]=((m_el[0].z() - m_el[2].x()) * s); + temp[2]=((m_el[1].x() - m_el[0].y()) * s); } else { - int i = m_el[0][0] < m_el[1][1] ? - (m_el[1][1] < m_el[2][2] ? 2 : 1) : - (m_el[0][0] < m_el[2][2] ? 2 : 0); + int i = m_el[0].x() < m_el[1].y() ? + (m_el[1].y() < m_el[2].z() ? 2 : 1) : + (m_el[0].x() < m_el[2].z() ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0)); - q[i] = s * btScalar(0.5); + temp[i] = s * btScalar(0.5); s = btScalar(0.5) / s; - q[3] = (m_el[k][j] - m_el[j][k]) * s; - q[j] = (m_el[j][i] + m_el[i][j]) * s; - q[k] = (m_el[k][i] + m_el[i][k]) * s; + temp[3] = (m_el[k][j] - m_el[j][k]) * s; + temp[j] = (m_el[j][i] + m_el[i][j]) * s; + temp[k] = (m_el[k][i] + m_el[i][k]) * s; } + q.setValue(temp[0],temp[1],temp[2],temp[3]); } - - void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const { - pitch = btScalar(btAsin(-m_el[2][0])); - if (pitch < SIMD_2_PI) + + if (btScalar(m_el[1].z()) < btScalar(1)) { - if (pitch > SIMD_2_PI) + if (btScalar(m_el[1].z()) > -btScalar(1)) { - yaw = btScalar(btAtan2(m_el[1][0], m_el[0][0])); - roll = btScalar(btAtan2(m_el[2][1], m_el[2][2])); + yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); + pitch = btScalar(btAsin(-m_el[1].y())); + roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); } else { - yaw = btScalar(-btAtan2(-m_el[0][1], m_el[0][2])); + yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z())); + pitch = SIMD_HALF_PI; roll = btScalar(0.0); } } else { - yaw = btScalar(btAtan2(-m_el[0][1], m_el[0][2])); + yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z())); + pitch = -SIMD_HALF_PI; roll = btScalar(0.0); } } - - btVector3 getScaling() const - { - return btVector3(m_el[0][0] * m_el[0][0] + m_el[1][0] * m_el[1][0] + m_el[2][0] * m_el[2][0], - m_el[0][1] * m_el[0][1] + m_el[1][1] * m_el[1][1] + m_el[2][1] * m_el[2][1], - m_el[0][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]); - } + + btMatrix3x3 scaled(const btVector3& s) const { - return btMatrix3x3(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2], - m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2], - m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][2] * s[2]); + return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), + m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), + m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); } btScalar determinant() const; @@ -263,11 +252,21 @@ class btMatrix3x3 { btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; - btScalar tdot(int c, const btVector3& v) const + SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const + { + return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); + } + SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const + { + return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); + } + SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const { - return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2]; + return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); } + + protected: btScalar cofac(int r1, int c1, int r2, int c2) const { @@ -280,9 +279,9 @@ class btMatrix3x3 { SIMD_FORCE_INLINE btMatrix3x3& btMatrix3x3::operator*=(const btMatrix3x3& m) { - setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]), - m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]), - m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2])); + setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), + m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), + m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); return *this; } @@ -297,17 +296,17 @@ class btMatrix3x3 { btMatrix3x3::absolute() const { return btMatrix3x3( - btFabs(m_el[0][0]), btFabs(m_el[0][1]), btFabs(m_el[0][2]), - btFabs(m_el[1][0]), btFabs(m_el[1][1]), btFabs(m_el[1][2]), - btFabs(m_el[2][0]), btFabs(m_el[2][1]), btFabs(m_el[2][2])); + btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), + btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), + btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); } SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::transpose() const { - return btMatrix3x3(m_el[0][0], m_el[1][0], m_el[2][0], - m_el[0][1], m_el[1][1], m_el[2][1], - m_el[0][2], m_el[1][2], m_el[2][2]); + return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), + m_el[0].y(), m_el[1].y(), m_el[2].y(), + m_el[0].z(), m_el[1].z(), m_el[2].z()); } SIMD_FORCE_INLINE btMatrix3x3 @@ -323,26 +322,26 @@ class btMatrix3x3 { { btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); btScalar det = (*this)[0].dot(co); - assert(det != btScalar(0.0f)); - btScalar s = btScalar(1.0f) / det; - return btMatrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, - co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, - co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); + btFullAssert(det != btScalar(0.0)); + btScalar s = btScalar(1.0) / det; + return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, + co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, + co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); } SIMD_FORCE_INLINE btMatrix3x3 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const { return btMatrix3x3( - m_el[0][0] * m[0][0] + m_el[1][0] * m[1][0] + m_el[2][0] * m[2][0], - m_el[0][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1], - m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2], - m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0], - m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1], - m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2], - m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0], - m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1], - m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]); + m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), + m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), + m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), + m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), + m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), + m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), + m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), + m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), + m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].x()); } SIMD_FORCE_INLINE btMatrix3x3 @@ -365,19 +364,19 @@ class btMatrix3x3 { SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v, const btMatrix3x3& m) { - return btVector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v)); + return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); } SIMD_FORCE_INLINE btMatrix3x3 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) { return btMatrix3x3( - m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]), - m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]), - m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2])); + m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), + m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), + m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); } - +/* SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { return btMatrix3x3( m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], @@ -390,6 +389,7 @@ class btMatrix3x3 { m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); } +*/ #endif diff --git a/extern/bullet2/src/LinearMath/btMotionState.h b/extern/bullet2/src/LinearMath/btMotionState.h index a9e212d3c71..1975e5ff900 100644 --- a/extern/bullet2/src/LinearMath/btMotionState.h +++ b/extern/bullet2/src/LinearMath/btMotionState.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef BT_MOTIONSTATE_H #define BT_MOTIONSTATE_H -#include "LinearMath/btTransform.h" +#include "btTransform.h" ///btMotionState allows the dynamics world to synchronize the updated world transforms with graphics ///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation) @@ -34,10 +34,7 @@ class btMotionState //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/btPoint3.h b/extern/bullet2/src/LinearMath/btPoint3.h index 4be7e9015bb..a2020e26d12 100644 --- a/extern/bullet2/src/LinearMath/btPoint3.h +++ b/extern/bullet2/src/LinearMath/btPoint3.h @@ -17,7 +17,7 @@ subject to the following restrictions: #ifndef btPoint3_H #define btPoint3_H -#include "LinearMath/btVector3.h" +#include "btVector3.h" typedef btVector3 btPoint3; diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet2/src/LinearMath/btQuadWord.h index 4331b668210..961ac484d20 100644 --- a/extern/bullet2/src/LinearMath/btQuadWord.h +++ b/extern/bullet2/src/LinearMath/btQuadWord.h @@ -16,12 +16,13 @@ subject to the following restrictions: #ifndef SIMD_QUADWORD_H #define SIMD_QUADWORD_H -#include "LinearMath/btScalar.h" +#include "btScalar.h" -ATTRIBUTE_ALIGNED16 (class btQuadWord) +///btQuadWord is base-class for vectors, points +class btQuadWord { protected: btScalar m_x; @@ -31,8 +32,8 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord) public: - SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } - SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; } +// SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } +// SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; } SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; } @@ -40,11 +41,13 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord) SIMD_FORCE_INLINE const btScalar& getZ() const { return m_z; } - SIMD_FORCE_INLINE void setX(float x) { m_x = x;}; + SIMD_FORCE_INLINE void setX(btScalar x) { m_x = x;}; - SIMD_FORCE_INLINE void setY(float y) { m_y = y;}; + SIMD_FORCE_INLINE void setY(btScalar y) { m_y = y;}; - SIMD_FORCE_INLINE void setZ(float z) { m_z = z;}; + SIMD_FORCE_INLINE void setZ(btScalar z) { m_z = z;}; + + SIMD_FORCE_INLINE void setW(btScalar w) { m_unusedW = w;}; SIMD_FORCE_INLINE const btScalar& x() const { return m_x; } @@ -52,15 +55,18 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord) SIMD_FORCE_INLINE const btScalar& z() const { return m_z; } + SIMD_FORCE_INLINE const btScalar& w() const { return m_unusedW; } + - operator btScalar *() { return &m_x; } - operator const btScalar *() const { return &m_x; } + SIMD_FORCE_INLINE operator btScalar *() { return &m_x; } + SIMD_FORCE_INLINE operator const btScalar *() const { return &m_x; } SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) { m_x=x; m_y=y; m_z=z; + m_unusedW = 0.f; } /* void getValue(btScalar *m) const @@ -78,15 +84,15 @@ ATTRIBUTE_ALIGNED16 (class btQuadWord) m_unusedW=w; } - SIMD_FORCE_INLINE btQuadWord() : - m_x(0.f),m_y(0.f),m_z(0.f),m_unusedW(0.f) + SIMD_FORCE_INLINE btQuadWord() + // :m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.)) { } SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z) :m_x(x),m_y(y),m_z(z) //todo, remove this in release/simd ? - ,m_unusedW(0.f) + ,m_unusedW(btScalar(0.)) { } diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index aec25a54955..d5a7913b742 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -17,7 +17,7 @@ subject to the following restrictions: #ifndef SIMD__QUATERNION_H_ #define SIMD__QUATERNION_H_ -#include "LinearMath/btVector3.h" +#include "btVector3.h" class btQuaternion : public btQuadWord { public: @@ -68,13 +68,13 @@ public: btQuaternion& operator+=(const btQuaternion& q) { - m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3]; + m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW; return *this; } btQuaternion& operator-=(const btQuaternion& q) { - m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3]; + m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW; return *this; } @@ -87,16 +87,16 @@ public: btQuaternion& operator*=(const btQuaternion& q) { - setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(), - m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(), - m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(), - m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z()); + setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(), + m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(), + m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(), + m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z()); return *this; } btScalar dot(const btQuaternion& q) const { - return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3]; + return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW; } btScalar length2() const @@ -150,7 +150,7 @@ public: btScalar getAngle() const { - btScalar s = 2.f * btAcos(m_unusedW); + btScalar s = btScalar(2.) * btAcos(m_unusedW); return s; } @@ -165,20 +165,20 @@ public: operator+(const btQuaternion& q2) const { const btQuaternion& q1 = *this; - return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]); + return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW); } SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q2) const { const btQuaternion& q1 = *this; - return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]); + return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW); } SIMD_FORCE_INLINE btQuaternion operator-() const { const btQuaternion& q2 = *this; - return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2[3]); + return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_unusedW); } SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const @@ -202,7 +202,7 @@ public: return btQuaternion((m_x * s0 + q.x() * s1) * d, (m_y * s0 + q.y() * s1) * d, (m_z * s0 + q.z() * s1) * d, - (m_unusedW * s0 + q[3] * s1) * d); + (m_unusedW * s0 + q.m_unusedW * s1) * d); } else { @@ -219,7 +219,7 @@ public: SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q) { - return btQuaternion(-q.x(), -q.y(), -q.z(), -q[3]); + return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w()); } @@ -227,27 +227,27 @@ operator-(const btQuaternion& q) SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q1, const btQuaternion& q2) { - return btQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(), - q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(), - q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(), - q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); + return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), + q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(), + q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(), + q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); } SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q, const btVector3& w) { - return btQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(), - q[3] * w.y() + q.z() * w.x() - q.x() * w.z(), - q[3] * w.z() + q.x() * w.y() - q.y() * w.x(), + return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), + q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), + q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); } SIMD_FORCE_INLINE btQuaternion operator*(const btVector3& w, const btQuaternion& q) { - return btQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(), - w.y() * q[3] + w.z() * q.x() - w.x() * q.z(), - w.z() * q[3] + w.x() * q.y() - w.y() * q.x(), + return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), + w.y() * q.w() + w.z() * q.x() - w.x() * q.z(), + w.z() * q.w() + w.x() * q.y() - w.y() * q.x(), -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); } diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp index 4d013a65853..37a0c8c3be5 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.cpp +++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: // Note: We must declare these private static variables again here to // avoid link errors. bool btProfiler::mEnabled = false; -hidden::Clock btProfiler::mClock; +btClock btProfiler::mClock; unsigned long int btProfiler::mCurrentCycleStartMicroseconds = 0; unsigned long int btProfiler::mLastCycleDurationMicroseconds = 0; std::map btProfiler::mProfileBlocks; diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h index ec560c8e33e..9389fcd8156 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -22,16 +22,16 @@ subject to the following restrictions: #ifndef QUICK_PROF_H #define QUICK_PROF_H -#define USE_QUICKPROF 1 +#include "btScalar.h" -#ifdef USE_QUICKPROF +//#define USE_QUICKPROF 1 +//Don't use quickprof for now, because it contains STL. TODO: replace STL by Bullet container classes. -#include -#include -#include -#include +//if you don't need btClock, you can comment next line +#define USE_BT_CLOCK 1 +#ifdef USE_BT_CLOCK #ifdef __PPU__ #include #include @@ -43,7 +43,12 @@ typedef uint64_t __int64; #endif #if defined(WIN32) || defined(_WIN32) - #define USE_WINDOWS_TIMERS + + #define USE_WINDOWS_TIMERS + #define WIN32_LEAN_AND_MEAN + #define NOWINRES + #define NOMCX + #define NOIME #include #include #else @@ -51,39 +56,12 @@ typedef uint64_t __int64; #endif #define mymin(a,b) (a > b ? a : b) -namespace hidden -{ - /// A simple data structure representing a single timed block - /// of code. - struct ProfileBlock - { - ProfileBlock() - { - currentBlockStartMicroseconds = 0; - currentCycleTotalMicroseconds = 0; - lastCycleTotalMicroseconds = 0; - totalMicroseconds = 0; - } - /// The starting time (in us) of the current block update. - unsigned long int currentBlockStartMicroseconds; - - /// The accumulated time (in us) spent in this block during the - /// current profiling cycle. - unsigned long int currentCycleTotalMicroseconds; - - /// The accumulated time (in us) spent in this block during the - /// past profiling cycle. - unsigned long int lastCycleTotalMicroseconds; - - /// The total accumulated time (in us) spent in this block. - unsigned long int totalMicroseconds; - }; - - class Clock +/// basic clock +class btClock { public: - Clock() + btClock() { #ifdef USE_WINDOWS_TIMERS QueryPerformanceFrequency(&mClockFrequency); @@ -91,7 +69,7 @@ namespace hidden reset(); } - ~Clock() + ~btClock() { } @@ -111,14 +89,14 @@ namespace hidden __asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); mStartTime = newTime; #else - gettimeofday(&mStartTime, NULL); + gettimeofday(&mStartTime, 0); #endif #endif } /// Returns the time in ms since the last call to reset or since - /// the Clock was created. + /// the btClock was created. unsigned long int getTimeMilliseconds() { #ifdef USE_WINDOWS_TIMERS @@ -168,7 +146,7 @@ namespace hidden #else struct timeval currentTime; - gettimeofday(¤tTime, NULL); + gettimeofday(¤tTime, 0); return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 + (currentTime.tv_usec - mStartTime.tv_usec) / 1000; #endif //__PPU__ @@ -226,7 +204,7 @@ namespace hidden #else struct timeval currentTime; - gettimeofday(¤tTime, NULL); + gettimeofday(¤tTime, 0); return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 + (currentTime.tv_usec - mStartTime.tv_usec); #endif//__PPU__ @@ -248,6 +226,50 @@ namespace hidden #endif //__PPU__ }; + +#endif //USE_BT_CLOCK + + +#ifdef USE_QUICKPROF + + +#include +#include +#include +#include + + + + +namespace hidden +{ + /// A simple data structure representing a single timed block + /// of code. + struct ProfileBlock + { + ProfileBlock() + { + currentBlockStartMicroseconds = 0; + currentCycleTotalMicroseconds = 0; + lastCycleTotalMicroseconds = 0; + totalMicroseconds = 0; + } + + /// The starting time (in us) of the current block update. + unsigned long int currentBlockStartMicroseconds; + + /// The accumulated time (in us) spent in this block during the + /// current profiling cycle. + unsigned long int currentCycleTotalMicroseconds; + + /// The accumulated time (in us) spent in this block during the + /// past profiling cycle. + unsigned long int lastCycleTotalMicroseconds; + + /// The total accumulated time (in us) spent in this block. + unsigned long int totalMicroseconds; + }; + }; /// A static class that manages timing for a set of profiling blocks. @@ -336,6 +358,7 @@ public: /// Prints an error message to standard output. inline static void printError(const std::string& msg) { + //btAssert(0); std::cout << "[QuickProf error] " << msg << std::endl; } @@ -343,7 +366,7 @@ public: static bool mEnabled; /// The clock used to time profile blocks. - static hidden::Clock mClock; + static btClock mClock; /// The starting time (in us) of the current profiling cycle. static unsigned long int mCurrentCycleStartMicroseconds; diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index dd76fb2de1a..222315c1c74 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -25,10 +25,11 @@ subject to the following restrictions: #ifdef WIN32 - #if defined(__MINGW32__) || defined(__CYGWIN__) + #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) #define SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a #else + #define BT_HAS_ALIGNED_ALOCATOR #pragma warning(disable:4530) #pragma warning(disable:4996) #pragma warning(disable:4786) @@ -38,8 +39,21 @@ subject to the following restrictions: #include #define btAssert assert + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) #else +#if defined (__CELLOS_LV2__) + #define SIMD_FORCE_INLINE inline + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #ifndef assert + #include + #endif + #define btAssert assert + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) +#else + //non-windows systems #define SIMD_FORCE_INLINE inline @@ -48,16 +62,28 @@ subject to the following restrictions: #include #endif #define btAssert assert + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) +#endif //__CELLOS_LV2__ #endif +/// older compilers (gcc 3.x) and Sun needs double version of sqrt etc. +/// exclude Apple Intel (i's assumed to be a Macbook or new Intel Dual Core Processor) +#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__)) +//use slow double float precision operation on those platforms +#ifndef BT_USE_DOUBLE_PRECISION +#define BT_FORCE_DOUBLE_FUNCTIONS +#endif +#endif +#if defined(BT_USE_DOUBLE_PRECISION) +typedef double btScalar; +#else +typedef float btScalar; +#endif -typedef float btScalar; -///older compilers (gcc 3.x) and Sun needs double versions of srqt etc. -///exclude Apple Intel (it's assumed to be a Macbook or newer Intel Dual Core processor) -#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__)) -//use slow double float precision operation on those platforms +#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS) SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); } SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); } @@ -90,13 +116,19 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } #endif -#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_2_PI btScalar(6.283185307179586232) +#define SIMD_PI (SIMD_2_PI * btScalar(0.5)) +#define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25)) +#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) +#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) + +#ifdef BT_USE_DOUBLE_PRECISION +#define SIMD_EPSILON DBL_EPSILON +#define SIMD_INFINITY DBL_MAX +#else #define SIMD_EPSILON FLT_EPSILON #define SIMD_INFINITY FLT_MAX +#endif SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } @@ -117,7 +149,7 @@ SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y) */ SIMD_FORCE_INLINE int btIsNegative(btScalar x) { - return x < 0.0f ? 1 : 0; + return x < btScalar(0.0) ? 1 : 0; } SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; } diff --git a/extern/bullet2/src/LinearMath/btSimdMinMax.h b/extern/bullet2/src/LinearMath/btSimdMinMax.h index 2731c8b09f3..75e83f3c53f 100644 --- a/extern/bullet2/src/LinearMath/btSimdMinMax.h +++ b/extern/bullet2/src/LinearMath/btSimdMinMax.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef SIMD_MINMAX_H #define SIMD_MINMAX_H -#include "LinearMath/btScalar.h" +#include "btScalar.h" template SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) { diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h index 3f9a48407c7..b1f3dfca492 100644 --- a/extern/bullet2/src/LinearMath/btTransform.h +++ b/extern/bullet2/src/LinearMath/btTransform.h @@ -17,8 +17,8 @@ subject to the following restrictions: #ifndef btTransform_H #define btTransform_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" +#include "btVector3.h" +#include "btMatrix3x3.h" ///btTransform supports rigid transforms (only translation and rotation, no scaling/shear) @@ -48,17 +48,19 @@ public: m_origin = t1(t2.m_origin); } - void multInverseLeft(const btTransform& t1, const btTransform& t2) { +/* void multInverseLeft(const btTransform& t1, const btTransform& t2) { btVector3 v = t2.m_origin - t1.m_origin; m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis); m_origin = v * t1.m_basis; } + */ + SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const { - return btVector3(m_basis[0].dot(x) + m_origin[0], - m_basis[1].dot(x) + m_origin[1], - m_basis[2].dot(x) + m_origin[2]); + return btVector3(m_basis[0].dot(x) + m_origin.x(), + m_basis[1].dot(x) + m_origin.y(), + m_basis[2].dot(x) + m_origin.z()); } SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const @@ -88,18 +90,16 @@ public: void setFromOpenGLMatrix(const btScalar *m) { m_basis.setFromOpenGLSubMatrix(m); - m_origin[0] = m[12]; - m_origin[1] = m[13]; - m_origin[2] = m[14]; + m_origin.setValue(m[12],m[13],m[14]); } void getOpenGLMatrix(btScalar *m) const { m_basis.getOpenGLSubMatrix(m); - m[12] = m_origin[0]; - m[13] = m_origin[1]; - m[14] = m_origin[2]; - m[15] = btScalar(1.0f); + m[12] = m_origin.x(); + m[13] = m_origin.y(); + m[14] = m_origin.z(); + m[15] = btScalar(1.0); } SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index 39fa830f4df..bc42fd166b6 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -16,48 +16,40 @@ subject to the following restrictions: #ifndef SIMD_TRANSFORM_UTIL_H #define SIMD_TRANSFORM_UTIL_H -#include "LinearMath/btTransform.h" -#define ANGULAR_MOTION_THRESHOLD 0.5f*SIMD_HALF_PI +#include "btTransform.h" +#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) -#define btRecipSqrt(x) ((float)(1.0f/btSqrt(float(x)))) /* reciprocal square root */ +#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) { - return btVector3(supportDir.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), - supportDir.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), - supportDir.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), + supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), + supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); } inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) { - if (btFabs(n[2]) > SIMDSQRT12) { + if (btFabs(n.z()) > SIMDSQRT12) { // choose p in y-z plane btScalar a = n[1]*n[1] + n[2]*n[2]; btScalar k = btRecipSqrt (a); - p[0] = 0; - p[1] = -n[2]*k; - p[2] = n[1]*k; + p.setValue(0,-n[2]*k,n[1]*k); // set q = n x p - q[0] = a*k; - q[1] = -n[0]*p[2]; - q[2] = n[0]*p[1]; + q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); } else { // choose p in x-y plane - btScalar a = n[0]*n[0] + n[1]*n[1]; + btScalar a = n.x()*n.x() + n.y()*n.y(); btScalar k = btRecipSqrt (a); - p[0] = -n[1]*k; - p[1] = n[0]*k; - p[2] = 0; + p.setValue(-n.y()*k,n.x()*k,0); // set q = n x p - q[0] = -n[2]*p[1]; - q[1] = n[2]*p[0]; - q[2] = a*k; + q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); } } @@ -74,9 +66,9 @@ public: predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep); // #define QUATERNION_DERIVATIVE #ifdef QUATERNION_DERIVATIVE - btQuaternion orn = curTrans.getRotation(); - orn += (angvel * orn) * (timeStep * 0.5f); - orn.normalize(); + btQuaternion predictedOrn = curTrans.getRotation(); + predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); + predictedOrn.normalize(); #else //exponential map btVector3 axis; @@ -87,20 +79,21 @@ public: fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; } - if ( fAngle < 0.001f ) + if ( fAngle < btScalar(0.001) ) { // use Taylor's expansions of sync function - axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle ); + axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle ); } else { // sync(fAngle) = sin(c*fAngle)/t - axis = angvel*( btSin(0.5f*fAngle*timeStep)/fAngle ); + axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle ); } - btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*0.5f )); + btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) )); btQuaternion orn0 = curTrans.getRotation(); btQuaternion predictedOrn = dorn * orn0; + predictedOrn.normalize(); #endif predictedTransform.setRotation(predictedOrn); } @@ -130,11 +123,11 @@ public: angle = dorn.getAngle(); axis = btVector3(dorn.x(),dorn.y(),dorn.z()); - axis[3] = 0.f; + axis[3] = btScalar(0.); //check for axis length btScalar len = axis.length2(); if (len < SIMD_EPSILON*SIMD_EPSILON) - axis = btVector3(1.f,0.f,0.f); + axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); else axis /= btSqrt(len); } diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 5a35652ecd3..74d41ad2a19 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -19,11 +19,10 @@ subject to the following restrictions: #include "btQuadWord.h" - -///btVector3 is 16byte aligned, and has an extra unused component m_w -///this extra component can be used by derived classes (Quaternion?) or by user -class btVector3 : public btQuadWord { - +///btVector3 can be used to represent 3D points and vectors. +///It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user +///Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers +class btVector3 : public btQuadWord { public: SIMD_FORCE_INLINE btVector3() {} @@ -31,7 +30,7 @@ public: SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) - :btQuadWord(x,y,z,0.f) + :btQuadWord(x,y,z,btScalar(0.)) { } @@ -64,7 +63,7 @@ public: SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) { - assert(s != btScalar(0.0)); + btFullAssert(s != btScalar(0.0)); return *this *= btScalar(1.0) / s; } @@ -99,7 +98,7 @@ public: SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const { btScalar s = btSqrt(length2() * v.length2()); - assert(s != btScalar(0.0)); + btFullAssert(s != btScalar(0.0)); return btAcos(dot(v) / s); } @@ -148,10 +147,10 @@ public: SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt) { - btScalar s = 1.0f - rt; - m_x = s * v0[0] + rt * v1.x(); - m_y = s * v0[1] + rt * v1.y(); - m_z = s * v0[2] + rt * v1.z(); + btScalar s = btScalar(1.0) - rt; + m_x = s * v0.x() + rt * v1.x(); + m_y = s * v0.y() + rt * v1.y(); + m_z = s * v0.z() + rt * v1.z(); //don't do the unused w component // m_co[3] = s * v0[3] + rt * v1[3]; } @@ -213,7 +212,7 @@ operator*(const btScalar& s, const btVector3& v) SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v, const btScalar& s) { - assert(s != btScalar(0.0)); + btFullAssert(s != btScalar(0.0)); return v * (btScalar(1.0) / s); } @@ -271,7 +270,7 @@ lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) { - return p1[0] == p2[0] && p1[1] == p2[1] && p1[2] == p2[2]; + return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z(); } SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const @@ -327,13 +326,13 @@ public: - float getW() const { return m_unusedW;} + btScalar getW() const { return m_unusedW;} SIMD_FORCE_INLINE int maxAxis4() const { int maxIndex = -1; - float maxVal = -1e30f; + btScalar maxVal = btScalar(-1e30); if (m_x > maxVal) { maxIndex = 0; @@ -366,7 +365,7 @@ public: SIMD_FORCE_INLINE int minAxis4() const { int minIndex = -1; - float minVal = 1e30f; + btScalar minVal = btScalar(1e30); if (m_x < minVal) { minIndex = 0; diff --git a/extern/bullet2/src/btBulletCollisionCommon.h b/extern/bullet2/src/btBulletCollisionCommon.h index 834dca09e47..8417ccc671f 100644 --- a/extern/bullet2/src/btBulletCollisionCommon.h +++ b/extern/bullet2/src/btBulletCollisionCommon.h @@ -25,6 +25,7 @@ subject to the following restrictions: ///Collision Shapes #include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" #include "BulletCollision/CollisionShapes/btCylinderShape.h" #include "BulletCollision/CollisionShapes/btConeShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" -- cgit v1.2.3