Finally upgraded to latest Bullet subversion, about to release 2.71. Some recent...
authorErwin Coumans <blender@erwincoumans.com>
Wed, 3 Sep 2008 02:27:16 +0000 (02:27 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Wed, 3 Sep 2008 02:27:16 +0000 (02:27 +0000)
HELP BUILD SYSTEM MAINTAINERS: Please help with updating all build systems: the newly added files need to be added. Note that the src/SoftBody has been added for future extension of real-time soft bodies.

221 files changed:
extern/bullet2/src/Bullet-C-Api.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h
extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
extern/bullet2/src/BulletSoftBody/CMakeLists.txt [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftBody.cpp [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftBody.h [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSparseSDF.h [new file with mode: 0644]
extern/bullet2/src/LinearMath/btAabbUtil2.h
extern/bullet2/src/LinearMath/btAlignedAllocator.cpp
extern/bullet2/src/LinearMath/btAlignedAllocator.h
extern/bullet2/src/LinearMath/btAlignedObjectArray.h
extern/bullet2/src/LinearMath/btDefaultMotionState.h
extern/bullet2/src/LinearMath/btGeometryUtil.cpp
extern/bullet2/src/LinearMath/btGeometryUtil.h
extern/bullet2/src/LinearMath/btIDebugDraw.h
extern/bullet2/src/LinearMath/btMatrix3x3.h
extern/bullet2/src/LinearMath/btMinMax.h
extern/bullet2/src/LinearMath/btMotionState.h
extern/bullet2/src/LinearMath/btQuadWord.h
extern/bullet2/src/LinearMath/btQuaternion.h
extern/bullet2/src/LinearMath/btQuickprof.cpp
extern/bullet2/src/LinearMath/btQuickprof.h
extern/bullet2/src/LinearMath/btScalar.h
extern/bullet2/src/LinearMath/btStackAlloc.h
extern/bullet2/src/LinearMath/btTransform.h
extern/bullet2/src/LinearMath/btTransformUtil.h
extern/bullet2/src/LinearMath/btVector3.h
extern/bullet2/src/btBulletCollisionCommon.h
extern/bullet2/src/btBulletDynamicsCommon.h
source/gameengine/Converter/KX_BlenderSceneConverter.cpp
source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h

index 078dcae63bb720de1a6d9f9221668cc88cb9580a..a196f6417bc78a5307b34116488c6be5fa3667e8 100644 (file)
@@ -23,11 +23,145 @@ subject to the following restrictions:
 #ifndef BULLET_C_API_H
 #define BULLET_C_API_H
 
+#define PL_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
+
+#ifdef BT_USE_DOUBLE_PRECISION
+typedef double plReal;
+#else
+typedef float  plReal;
+#endif
+
+typedef plReal plVector3[3];
+typedef plReal plQuaternion[4];
+
 #ifdef __cplusplus
 extern "C" { 
 #endif
 
-double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3]);
+/*     Particular physics SDK */
+       PL_DECLARE_HANDLE(plPhysicsSdkHandle);
+
+/*     Dynamics world, belonging to some physics SDK */
+       PL_DECLARE_HANDLE(plDynamicsWorldHandle);
+
+/* Rigid Body that can be part of a Dynamics World */  
+       PL_DECLARE_HANDLE(plRigidBodyHandle);
+
+/*     Collision Shape/Geometry, property of a Rigid Body */
+       PL_DECLARE_HANDLE(plCollisionShapeHandle);
+
+/* Constraint for Rigid Bodies */
+       PL_DECLARE_HANDLE(plConstraintHandle);
+
+/* Triangle Mesh interface */
+       PL_DECLARE_HANDLE(plMeshInterfaceHandle);
+
+/* Broadphase Scene/Proxy Handles */
+       PL_DECLARE_HANDLE(plCollisionBroadphaseHandle);
+       PL_DECLARE_HANDLE(plBroadphaseProxyHandle);
+       PL_DECLARE_HANDLE(plCollisionWorldHandle);
+
+/*
+       Create and Delete a Physics SDK 
+*/
+
+       extern  plPhysicsSdkHandle      plNewBulletSdk(); //this could be also another sdk, like ODE, PhysX etc.
+       extern  void            plDeletePhysicsSdk(plPhysicsSdkHandle   physicsSdk);
+
+/* Collision World, not strictly necessary, you can also just create a Dynamics World with Rigid Bodies which internally manages the Collision World with Collision Objects */
+
+       typedef void(*btBroadphaseCallback)(void* clientData, void* object1,void* object2);
+
+       extern plCollisionBroadphaseHandle      plCreateSapBroadphase(btBroadphaseCallback beginCallback,btBroadphaseCallback endCallback);
+
+       extern void     plDestroyBroadphase(plCollisionBroadphaseHandle bp);
+
+       extern  plBroadphaseProxyHandle plCreateProxy(plCollisionBroadphaseHandle bp, void* clientData, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ);
+
+       extern void plDestroyProxy(plCollisionBroadphaseHandle bp, plBroadphaseProxyHandle proxyHandle);
+
+       extern void plSetBoundingBox(plBroadphaseProxyHandle proxyHandle, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ);
+
+/* todo: add pair cache support with queries like add/remove/find pair */
+       
+       extern plCollisionWorldHandle plCreateCollisionWorld(plPhysicsSdkHandle physicsSdk);
+
+/* todo: add/remove objects */
+       
+
+/* Dynamics World */
+
+       extern  plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdk);
+
+       extern  void           plDeleteDynamicsWorld(plDynamicsWorldHandle world);
+
+       extern  void    plStepSimulation(plDynamicsWorldHandle, plReal  timeStep);
+
+       extern  void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object);
+
+       extern  void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object);
+
+
+/* Rigid Body  */
+
+       extern  plRigidBodyHandle plCreateRigidBody(    void* user_data,  float mass, plCollisionShapeHandle cshape );
+
+       extern  void plDeleteRigidBody(plRigidBodyHandle body);
+
+
+/* Collision Shape definition */
+
+       extern  plCollisionShapeHandle plNewSphereShape(plReal radius);
+       extern  plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z);
+       extern  plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height); 
+       extern  plCollisionShapeHandle plNewConeShape(plReal radius, plReal height);
+       extern  plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height);
+       extern  plCollisionShapeHandle plNewCompoundShape();
+       extern  void    plAddChildShape(plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn);
+
+       extern  void plDeleteShape(plCollisionShapeHandle shape);
+
+       /* Convex Meshes */
+       extern  plCollisionShapeHandle plNewConvexHullShape();
+       extern  void            plAddVertex(plCollisionShapeHandle convexHull, plReal x,plReal y,plReal z);
+/* Concave static triangle meshes */
+       extern  plMeshInterfaceHandle              plNewMeshInterface();
+       extern  void            plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
+       extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
+
+       extern  void plSetScaling(plCollisionShapeHandle shape, plVector3 scaling);
+
+/* SOLID has Response Callback/Table/Management */
+/* PhysX has Triggers, User Callbacks and filtering */
+/* ODE has the typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2); */
+
+/*     typedef void plUpdatedPositionCallback(void* userData, plRigidBodyHandle        rbHandle, plVector3 pos); */
+/*     typedef void plUpdatedOrientationCallback(void* userData, plRigidBodyHandle     rbHandle, plQuaternion orientation); */
+
+       /* get world transform */
+       extern void     plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix);
+       extern void     plGetPosition(plRigidBodyHandle object,plVector3 position);
+       extern void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation);
+
+       /* set world transform (position/orientation) */
+       extern  void plSetPosition(plRigidBodyHandle object, const plVector3 position);
+       extern  void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation);
+       extern  void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient);
+
+       typedef struct plRayCastResult {
+               plRigidBodyHandle               m_body;  
+               plCollisionShapeHandle  m_shape;                
+               plVector3                               m_positionWorld;                
+               plVector3                               m_normalWorld;
+       } plRayCastResult;
+
+       extern  int plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plRayCastResult res);
+
+       /* Sweep API */
+
+       /* extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); */
+
+       /* Continuous Collision Detection API */
 
 #ifdef __cplusplus
 }
index be4a11506df48388820989d28d4b48a67d735e8a..d7eea33ea416d17b2c7dc8f5a373f1f29229daa6 100644 (file)
 
 #include <assert.h>
 
-#ifdef DEBUG_BROADPHASE
-#include <stdio.h>
-void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinality)
+btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache)
+:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache)
 {
-       int numEdges = m_pHandles[0].m_maxEdges[axis];
-       printf("SAP Axis %d, numEdges=%d\n",axis,numEdges);
-
-       int i;
-       for (i=0;i<numEdges+1;i++)
-       {
-               Edge* pEdge = m_pEdges[axis] + i;
-               Handle* pHandlePrev = getHandle(pEdge->m_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)
-{
-               (void)shapeType;
-               BP_FP_INT_TYPE handleId = addHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask);
-               
-               Handle* handle = getHandle(handleId);
-                               
-               return handle;
-}
-
-void   btAxisSweep3::destroyProxy(btBroadphaseProxy* proxy)
-{
-       Handle* handle = static_cast<Handle*>(proxy);
-       removeHandle(handle->m_handleId);
-}
-
-void   btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
-{
-       Handle* handle = static_cast<Handle*>(proxy);
-       updateHandle(handle->m_handleId,aabbMin,aabbMax);
-
-}
-
-
-
-
-
-
-btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles)
-:btOverlappingPairCache()
-{
-       m_invalidPair = 0;
-       //assert(bounds.HasVolume());
-
        // 1 handle is reserved as sentinel
-       btAssert(maxHandles > 1 && maxHandles < BP_MAX_HANDLES);
-
-       // init bounds
-       m_worldAabbMin = worldAabbMin;
-       m_worldAabbMax = worldAabbMax;
-
-       btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
-
-       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];
-       m_maxHandles = maxHandles;
-       m_numHandles = 0;
-
-       // handle 0 is reserved as the null index, and is also used as the sentinel
-       m_firstFreeHandle = 1;
-       {
-               for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
-                       m_pHandles[i].SetNextFree(i + 1);
-               m_pHandles[maxHandles - 1].SetNextFree(0);
-       }
-
-       {
-       // allocate edge buffers
-       for (int i = 0; i < 3; i++)
-               m_pEdges[i] = new Edge[maxHandles * 2];
-       }
-       //removed overlap management
-
-       // make boundary sentinels
-       
-       m_pHandles[0].m_clientObject = 0;
-
-       for (int axis = 0; axis < 3; axis++)
-       {
-               m_pHandles[0].m_minEdges[axis] = 0;
-               m_pHandles[0].m_maxEdges[axis] = 1;
-
-               m_pEdges[axis][0].m_pos = 0;
-               m_pEdges[axis][0].m_handle = 0;
-               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()
-{
-       
-       for (int i = 2; i >= 0; i--)
-               delete[] m_pEdges[i];
-       delete[] m_pHandles;
-}
-
-void btAxisSweep3::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
-{
-       btPoint3 clampedPoint(point);
-       
-
-
-       clampedPoint.setMax(m_worldAabbMin);
-       clampedPoint.setMin(m_worldAabbMax);
-
-       btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
-       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);
-       
-}
-
-
-
-BP_FP_INT_TYPE btAxisSweep3::allocHandle()
-{
-       assert(m_firstFreeHandle);
-
-       BP_FP_INT_TYPE handle = m_firstFreeHandle;
-       m_firstFreeHandle = getHandle(handle)->GetNextFree();
-       m_numHandles++;
-
-       return handle;
-}
-
-void btAxisSweep3::freeHandle(BP_FP_INT_TYPE handle)
-{
-       assert(handle > 0 && handle < m_maxHandles);
-
-       getHandle(handle)->SetNextFree(m_firstFreeHandle);
-       m_firstFreeHandle = handle;
-
-       m_numHandles--;
-}
-
-
-
-BP_FP_INT_TYPE btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask)
-{
-       // quantize the bounds
-       BP_FP_INT_TYPE min[3], max[3];
-       quantize(min, aabbMin, 0);
-       quantize(max, aabbMax, 1);
-
-       // allocate a handle
-       BP_FP_INT_TYPE handle = allocHandle();
-       assert(handle!= 0xcdcd);
-
-       Handle* pHandle = getHandle(handle);
-       
-       pHandle->m_handleId = handle;
-       //pHandle->m_pOverlaps = 0;
-       pHandle->m_clientObject = pOwner;
-       pHandle->m_collisionFilterGroup = collisionFilterGroup;
-       pHandle->m_collisionFilterMask = collisionFilterMask;
-
-       // compute current limit of edge arrays
-       BP_FP_INT_TYPE limit = m_numHandles * 2;
-
-       
-       // insert new edges just inside the max boundary edge
-       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];
-
-               m_pEdges[axis][limit - 1].m_pos = min[axis];
-               m_pEdges[axis][limit - 1].m_handle = handle;
-
-               m_pEdges[axis][limit].m_pos = max[axis];
-               m_pEdges[axis][limit].m_handle = handle;
-
-               pHandle->m_minEdges[axis] = limit - 1;
-               pHandle->m_maxEdges[axis] = limit;
-       }
-
-       // now sort the new edges to their correct position
-       sortMinDown(0, pHandle->m_minEdges[0], false);
-       sortMaxDown(0, pHandle->m_maxEdges[0], false);
-       sortMinDown(1, pHandle->m_minEdges[1], false);
-       sortMaxDown(1, pHandle->m_maxEdges[1], false);
-       sortMinDown(2, pHandle->m_minEdges[2], true);
-       sortMaxDown(2, pHandle->m_maxEdges[2], true);
-
-
-       return handle;
-}
-
-
-void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle)
-{
-       
-       Handle* pHandle = getHandle(handle);
-
-       //explicitly remove the pairs containing the proxy
-       //we could do it also in the sortMinUp (passing true)
-       //todo: compare performance
-       removeOverlappingPairsContainingProxy(pHandle);
-
-
-       // compute current limit of edge arrays
-       int limit = m_numHandles * 2;
-       
-       int axis;
-
-       for (axis = 0;axis<3;axis++)
-       {
-               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];
-               BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
-               pEdges[max].m_pos = BP_HANDLE_SENTINEL;
-
-               sortMaxUp(axis,max,false);
-
-
-               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 = 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;i<m_overlappingPairArray.size();i++)
-       {
-       
-               btBroadphasePair& pair = m_overlappingPairArray[i];
-
-               bool isDuplicate = (pair == previousPair);
-
-               previousPair = pair;
-
-               bool needsRemoval = false;
-
-               if (!isDuplicate)
-               {
-                       bool hasOverlap = testOverlap(pair.m_pProxy0,pair.m_pProxy1);
-
-                       if (hasOverlap)
-                       {
-                               needsRemoval = callback->processOverlap(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<Handle*>(proxy0);
-       const Handle* pHandleB = static_cast<Handle*>(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
-
-       for (int axis = 0; axis < 3; axis++)
-       { 
-               if (axis != ignoreAxis)
-               {
-                       if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
-                               pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
-                       { 
-                               return false; 
-                       } 
-               }
-       } 
-
-       //optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization)
-
-       /*for (int axis = 0; axis < 3; axis++)
-       {
-               if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos ||
-                       m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos)
-               {
-                       return false;
-               }
-       }
-       */
-
-       return true;
-}
-
-void btAxisSweep3::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax)
-{
-//     assert(bounds.IsFinite());
-       //assert(bounds.HasVolume());
-
-       Handle* pHandle = getHandle(handle);
-
-       // quantize the new bounds
-       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++)
-       {
-               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;
-
-               m_pEdges[axis][emin].m_pos = min[axis];
-               m_pEdges[axis][emax].m_pos = max[axis];
-
-               // expand (only adds overlaps)
-               if (dmin < 0)
-                       sortMinDown(axis, emin);
-
-               if (dmax > 0)
-                       sortMaxUp(axis, emax);
-
-               // shrink (only removes overlaps)
-               if (dmin > 0)
-                       sortMinUp(axis, emin);
-
-               if (dmax < 0)
-                       sortMaxDown(axis, emax);
-
-#ifdef DEBUG_BROADPHASE
-       debugPrintAxis(axis);
-#endif //DEBUG_BROADPHASE
-       }
-
-       
-}
-
-
-
-
-// sorting a min edge downwards can only ever *add* overlaps
-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);
-
-       while (pEdge->m_pos < pPrev->m_pos)
-       {
-               Handle* pHandlePrev = getHandle(pPrev->m_handle);
-
-               if (pPrev->IsMax())
-               {
-                       // if previous edge is a maximum check the bounds and add an overlap if necessary
-                       if (updateOverlaps && testOverlap(axis,pHandleEdge, pHandlePrev))
-                       {
-                               addOverlappingPair(pHandleEdge,pHandlePrev);
-
-                               //AddOverlap(pEdge->m_handle, pPrev->m_handle);
-
-                       }
-
-                       // update edge reference in other handle
-                       pHandlePrev->m_maxEdges[axis]++;
-               }
-               else
-                       pHandlePrev->m_minEdges[axis]++;
-
-               pHandleEdge->m_minEdges[axis]--;
-
-               // swap the edges
-               Edge swap = *pEdge;
-               *pEdge = *pPrev;
-               *pPrev = swap;
-
-               // decrement
-               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, BP_FP_INT_TYPE edge, bool updateOverlaps)
-{
-       Edge* pEdge = m_pEdges[axis] + edge;
-       Edge* pNext = pEdge + 1;
-       Handle* pHandleEdge = getHandle(pEdge->m_handle);
-
-       while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
-       {
-               Handle* pHandleNext = getHandle(pNext->m_handle);
-
-               if (pNext->IsMax())
-               {
-                       // 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);
-                               */
-
-                       }
-
-                       // update edge reference in other handle
-                       pHandleNext->m_maxEdges[axis]--;
-               }
-               else
-                       pHandleNext->m_minEdges[axis]--;
-
-               pHandleEdge->m_minEdges[axis]++;
-
-               // swap the edges
-               Edge swap = *pEdge;
-               *pEdge = *pNext;
-               *pNext = swap;
-
-               // increment
-               pEdge++;
-               pNext++;
-       }
-
+       btAssert(maxHandles > 1 && maxHandles < 32767);
 
 }
 
-// sorting a max edge downwards can only ever *remove* overlaps
-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);
-
-       while (pEdge->m_pos < pPrev->m_pos)
-       {
-               Handle* pHandlePrev = getHandle(pPrev->m_handle);
-
-               if (!pPrev->IsMax())
-               {
-                       // 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);
-                               //assert(pair);
-
-                               if (pair)
-                               {
-                                       removeOverlappingPair(*pair);
-                               }
-                               */
-
-                       }
-
-                       // update edge reference in other handle
-                       pHandlePrev->m_minEdges[axis]++;;
-               }
-               else
-                       pHandlePrev->m_maxEdges[axis]++;
-
-               pHandleEdge->m_maxEdges[axis]--;
-
-               // swap the edges
-               Edge swap = *pEdge;
-               *pEdge = *pPrev;
-               *pPrev = swap;
 
-               // decrement
-               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, BP_FP_INT_TYPE edge, bool updateOverlaps)
+bt32BitAxisSweep3::bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache )
+:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache)
 {
-       Edge* pEdge = m_pEdges[axis] + edge;
-       Edge* pNext = pEdge + 1;
-       Handle* pHandleEdge = getHandle(pEdge->m_handle);
-
-       while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
-       {
-               Handle* pHandleNext = getHandle(pNext->m_handle);
-
-               if (!pNext->IsMax())
-               {
-                       // if next edge is a minimum check the bounds and add an overlap if necessary
-                       if (updateOverlaps && testOverlap(axis, pHandleEdge, pHandleNext))
-                       {
-                               Handle* handle0 = getHandle(pEdge->m_handle);
-                               Handle* handle1 = getHandle(pNext->m_handle);
-                               addOverlappingPair(handle0,handle1);
-                       }
-
-                       // update edge reference in other handle
-                       pHandleNext->m_minEdges[axis]--;
-               }
-               else
-                       pHandleNext->m_maxEdges[axis]--;
-
-               pHandleEdge->m_maxEdges[axis]++;
-
-               // swap the edges
-               Edge swap = *pEdge;
-               *pEdge = *pNext;
-               *pNext = swap;
-
-               // increment
-               pEdge++;
-               pNext++;
-       }
-       
+       // 1 handle is reserved as sentinel
+       btAssert(maxHandles > 1 && maxHandles < 2147483647);
 }
index 57bbb36867225297c866ed1618b94193f61c6584..e7c5fb5b6cfe7fa4ccffa1cdd8b84ae6d5349925 100644 (file)
 #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 "btBroadphaseInterface.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
+#include "btOverlappingPairCallback.h"
 
 //#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
-class btAxisSweep3 : public btOverlappingPairCache
+/// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase.
+/// It uses quantized integers to represent the begin and end points for each of the 3 axis.
+/// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead.
+template <typename BP_FP_INT_TYPE>
+class btAxisSweep3Internal : public btBroadphaseInterface
 {
+protected:
+
+       BP_FP_INT_TYPE  m_bpHandleMask;
+       BP_FP_INT_TYPE  m_handleSentinel;
 
 public:
        
@@ -57,40 +48,52 @@ public:
                BP_FP_INT_TYPE m_pos;                   // low bit is min/max
                BP_FP_INT_TYPE m_handle;
 
-               BP_FP_INT_TYPE IsMax() const {return m_pos & 1;}
+               BP_FP_INT_TYPE IsMax() const {return static_cast<BP_FP_INT_TYPE>(m_pos & 1);}
        };
 
 public:
+       //This breaks the Intel compiler, see http://softwarecommunity.intel.com/isn/Community/en-US/forums/thread/30253577.aspx
        class Handle : public btBroadphaseProxy
+       //ATTRIBUTE_ALIGNED16(class) Handle : public btBroadphaseProxy
        {
        public:
-               
+       BT_DECLARE_ALIGNED_ALLOCATOR();
+       
                // indexes into the edge arrays
                BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];            // 6 * 2 = 12
-               BP_FP_INT_TYPE m_handleId;
+//             BP_FP_INT_TYPE m_uniqueId;
                BP_FP_INT_TYPE m_pad;
                
                //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
        
-               inline void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
-               inline BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
+               SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
+               SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
        };              // 24 bytes + 24 for Edge structures = 44 bytes total per entry
 
        
-private:
+protected:
        btPoint3 m_worldAabbMin;                                                // overall system bounds
        btPoint3 m_worldAabbMax;                                                // overall system bounds
 
        btVector3 m_quantize;                                           // scaling factor for quantization
 
        BP_FP_INT_TYPE m_numHandles;                                            // number of active handles
-       int m_maxHandles;                                               // max number of handles
+       BP_FP_INT_TYPE m_maxHandles;                                            // max number of handles
        Handle* m_pHandles;                                             // handles pool
+       void* m_pHandlesRawPtr;
        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)
+       void* m_pEdgesRawPtr[3];
+
+       btOverlappingPairCache* m_pairCache;
+
+       ///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
+       btOverlappingPairCallback* m_userPairCallback;
+       
+       bool    m_ownsPairCache;
 
-       int m_invalidPair;
+       int     m_invalidPair;
 
        // allocation/deallocation
        BP_FP_INT_TYPE allocHandle();
@@ -108,29 +111,801 @@ private:
 
        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);
+       void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+       void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+       void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+       void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
 
 public:
-       btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384);
-       virtual ~btAxisSweep3();
 
-       virtual void    refreshOverlappingPairs();
+       btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0);
+
+       virtual ~btAxisSweep3Internal();
+
+       BP_FP_INT_TYPE getNumHandles() const
+       {
+               return m_numHandles;
+       }
+
+       virtual void    calculateOverlappingPairs(btDispatcher* dispatcher);
        
-       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;}
+       BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+       void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
+       void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
+       SIMD_FORCE_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);
+       virtual btBroadphaseProxy*      createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+       virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+       virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+       
+       bool    testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+       btOverlappingPairCache* getOverlappingPairCache()
+       {
+               return m_pairCache;
+       }
+       const btOverlappingPairCache*   getOverlappingPairCache() const
+       {
+               return m_pairCache;
+       }
+
+       void    setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback)
+       {
+               m_userPairCallback = pairCallback;
+       }
+       const btOverlappingPairCallback*        getOverlappingPairUserCallback() const
+       {
+               return m_userPairCallback;
+       }
+
+       ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+       ///will add some transform later
+       virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+       {
+               aabbMin = m_worldAabbMin;
+               aabbMax = m_worldAabbMax;
+       }
+
+       virtual void    printStats()
+       {
+/*             printf("btAxisSweep3.h\n");
+               printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
+               printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(),
+                       m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ());
+                       */
+
+       }
+
+};
+
+////////////////////////////////////////////////////////////////////
+
+
+
+
+#ifdef DEBUG_BROADPHASE
+#include <stdio.h>
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3<BP_FP_INT_TYPE>::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;i<numEdges+1;i++)
+       {
+               Edge* pEdge = m_pEdges[axis] + i;
+               Handle* pHandlePrev = getHandle(pEdge->m_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
+
+template <typename BP_FP_INT_TYPE>
+btBroadphaseProxy*     btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
+{
+               (void)shapeType;
+               BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy);
+               
+               Handle* handle = getHandle(handleId);
+                               
+               return handle;
+}
+
+
+
+template <typename BP_FP_INT_TYPE>
+void   btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+       Handle* handle = static_cast<Handle*>(proxy);
+       removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
+}
+
+template <typename BP_FP_INT_TYPE>
+void   btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
+{
+       Handle* handle = static_cast<Handle*>(proxy);
+       updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
+
+}
+
+
+
+
+
+template <typename BP_FP_INT_TYPE>
+btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache )
+:m_bpHandleMask(handleMask),
+m_handleSentinel(handleSentinel),
+m_pairCache(pairCache),
+m_userPairCallback(0),
+m_ownsPairCache(false),
+m_invalidPair(0)
+{
+       BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle
+
+       if (!m_pairCache)
+       {
+               void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
+               m_pairCache = new(ptr) btHashedOverlappingPairCache();
+               m_ownsPairCache = true;
+       }
+
+       //assert(bounds.HasVolume());
+
+       // init bounds
+       m_worldAabbMin = worldAabbMin;
+       m_worldAabbMax = worldAabbMax;
+
+       btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
+
+       BP_FP_INT_TYPE  maxInt = m_handleSentinel;
+
+       m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
+
+       // allocate handles buffer and put all handles on free list
+       m_pHandlesRawPtr = btAlignedAlloc(sizeof(Handle)*maxHandles,16);
+       m_pHandles = new(m_pHandlesRawPtr) Handle[maxHandles];
+
+       m_maxHandles = maxHandles;
+       m_numHandles = 0;
+
+       // handle 0 is reserved as the null index, and is also used as the sentinel
+       m_firstFreeHandle = 1;
+       {
+               for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
+                       m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
+               m_pHandles[maxHandles - 1].SetNextFree(0);
+       }
+
+       {
+               // allocate edge buffers
+               for (int i = 0; i < 3; i++)
+               {
+                       m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16);
+                       m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2];
+               }
+       }
+       //removed overlap management
+
+       // make boundary sentinels
+       
+       m_pHandles[0].m_clientObject = 0;
+
+       for (int axis = 0; axis < 3; axis++)
+       {
+               m_pHandles[0].m_minEdges[axis] = 0;
+               m_pHandles[0].m_maxEdges[axis] = 1;
+
+               m_pEdges[axis][0].m_pos = 0;
+               m_pEdges[axis][0].m_handle = 0;
+               m_pEdges[axis][1].m_pos = m_handleSentinel;
+               m_pEdges[axis][1].m_handle = 0;
+#ifdef DEBUG_BROADPHASE
+               debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
+       }
+
+}
+
+template <typename BP_FP_INT_TYPE>
+btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
+{
+       
+       for (int i = 2; i >= 0; i--)
+       {
+               btAlignedFree(m_pEdgesRawPtr[i]);
+       }
+       btAlignedFree(m_pHandlesRawPtr);
+
+       if (m_ownsPairCache)
+       {
+               m_pairCache->~btOverlappingPairCache();
+               btAlignedFree(m_pairCache);
+       }
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
+{
+       btPoint3 clampedPoint(point);
+       
+
+
+       clampedPoint.setMax(m_worldAabbMin);
+       clampedPoint.setMin(m_worldAabbMax);
+
+       btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
+       out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax);
+       out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax);
+       out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax);
+       
+}
+
+
+template <typename BP_FP_INT_TYPE>
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
+{
+       assert(m_firstFreeHandle);
+
+       BP_FP_INT_TYPE handle = m_firstFreeHandle;
+       m_firstFreeHandle = getHandle(handle)->GetNextFree();
+       m_numHandles++;
+
+       return handle;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
+{
+       assert(handle > 0 && handle < m_maxHandles);
+
+       getHandle(handle)->SetNextFree(m_firstFreeHandle);
+       m_firstFreeHandle = handle;
+
+       m_numHandles--;
+}
+
+
+template <typename BP_FP_INT_TYPE>
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
+{
+       // quantize the bounds
+       BP_FP_INT_TYPE min[3], max[3];
+       quantize(min, aabbMin, 0);
+       quantize(max, aabbMax, 1);
+
+       // allocate a handle
+       BP_FP_INT_TYPE handle = allocHandle();
+       
+
+       Handle* pHandle = getHandle(handle);
+       
+       pHandle->m_uniqueId = static_cast<int>(handle);
+       //pHandle->m_pOverlaps = 0;
+       pHandle->m_clientObject = pOwner;
+       pHandle->m_collisionFilterGroup = collisionFilterGroup;
+       pHandle->m_collisionFilterMask = collisionFilterMask;
+       pHandle->m_multiSapParentProxy = multiSapProxy;
+
+       // compute current limit of edge arrays
+       BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2);
+
+       
+       // insert new edges just inside the max boundary edge
+       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];
+
+               m_pEdges[axis][limit - 1].m_pos = min[axis];
+               m_pEdges[axis][limit - 1].m_handle = handle;
+
+               m_pEdges[axis][limit].m_pos = max[axis];
+               m_pEdges[axis][limit].m_handle = handle;
+
+               pHandle->m_minEdges[axis] = static_cast<BP_FP_INT_TYPE>(limit - 1);
+               pHandle->m_maxEdges[axis] = limit;
+       }
+
+       // now sort the new edges to their correct position
+       sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false);
+       sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false);
+       sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false);
+       sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false);
+       sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true);
+       sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true);
+
+
+       return handle;
+}
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher)
+{
+
+       Handle* pHandle = getHandle(handle);
+
+       //explicitly remove the pairs containing the proxy
+       //we could do it also in the sortMinUp (passing true)
+       //todo: compare performance
+       if (!m_pairCache->hasDeferredRemoval())
+       {
+               m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher);
+       }
+
+       // compute current limit of edge arrays
+       int limit = static_cast<int>(m_numHandles * 2);
+       
+       int axis;
+
+       for (axis = 0;axis<3;axis++)
+       {
+               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];
+               BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
+               pEdges[max].m_pos = m_handleSentinel;
+
+               sortMaxUp(axis,max,dispatcher,false);
+
+
+               BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
+               pEdges[i].m_pos = m_handleSentinel;
+
+
+               sortMinUp(axis,i,dispatcher,false);
+
+               pEdges[limit-1].m_handle = 0;
+               pEdges[limit-1].m_pos = m_handleSentinel;
+               
+#ifdef DEBUG_BROADPHASE
+                       debugPrintAxis(axis,false);
+#endif //DEBUG_BROADPHASE
+
+
+       }
+
+
+       // free the handle
+       freeHandle(handle);
+
+       
+}
+
+extern int gOverlappingPairs;
+//#include <stdio.h>
+
+template <typename BP_FP_INT_TYPE>
+void   btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+
+       if (m_pairCache->hasDeferredRemoval())
+       {
+       
+               btBroadphasePairArray&  overlappingPairArray = m_pairCache->getOverlappingPairArray();
+
+               //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+               overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+               overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+               m_invalidPair = 0;
+
+               
+               int i;
+
+               btBroadphasePair previousPair;
+               previousPair.m_pProxy0 = 0;
+               previousPair.m_pProxy1 = 0;
+               previousPair.m_algorithm = 0;
+               
+               
+               for (i=0;i<overlappingPairArray.size();i++)
+               {
+               
+                       btBroadphasePair& pair = overlappingPairArray[i];
+
+                       bool isDuplicate = (pair == previousPair);
+
+                       previousPair = pair;
+
+                       bool needsRemoval = false;
+
+                       if (!isDuplicate)
+                       {
+                               bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+                               if (hasOverlap)
+                               {
+                                       needsRemoval = false;//callback->processOverlap(pair);
+                               } else
+                               {
+                                       needsRemoval = true;
+                               }
+                       } else
+                       {
+                               //remove duplicate
+                               needsRemoval = true;
+                               //should have no algorithm
+                               btAssert(!pair.m_algorithm);
+                       }
+                       
+                       if (needsRemoval)
+                       {
+                               m_pairCache->cleanOverlappingPair(pair,dispatcher);
+
+               //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+               //              m_overlappingPairArray.pop_back();
+                               pair.m_pProxy0 = 0;
+                               pair.m_pProxy1 = 0;
+                               m_invalidPair++;
+                               gOverlappingPairs--;
+                       } 
+                       
+               }
+
+       ///if you don't like to skip the invalid pairs in the array, execute following code:
+       #define CLEAN_INVALID_PAIRS 1
+       #ifdef CLEAN_INVALID_PAIRS
+
+               //perform a sort, to sort 'invalid' pairs to the end
+               overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+               overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+               m_invalidPair = 0;
+       #endif//CLEAN_INVALID_PAIRS
+               
+               //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
+       }
+
+
+
+       
+
+}
+
+
+template <typename BP_FP_INT_TYPE>
+bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+       const Handle* pHandleA = static_cast<Handle*>(proxy0);
+       const Handle* pHandleB = static_cast<Handle*>(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;
+}
+
+template <typename BP_FP_INT_TYPE>
+bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB)
+{
+       //optimization 1: check the array index (memory address), instead of the m_pos
+
+       for (int axis = 0; axis < 3; axis++)
+       { 
+               if (axis != ignoreAxis)
+               {
+                       if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
+                               pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
+                       { 
+                               return false; 
+                       } 
+               }
+       } 
+
+       //optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization)
+
+       /*for (int axis = 0; axis < 3; axis++)
+       {
+               if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos ||
+                       m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos)
+               {
+                       return false;
+               }
+       }
+       */
+
+       return true;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher)
+{
+//     assert(bounds.IsFinite());
+       //assert(bounds.HasVolume());
+
+       Handle* pHandle = getHandle(handle);
+
+       // quantize the new bounds
+       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++)
+       {
+               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;
+
+               m_pEdges[axis][emin].m_pos = min[axis];
+               m_pEdges[axis][emax].m_pos = max[axis];
+
+               // expand (only adds overlaps)
+               if (dmin < 0)
+                       sortMinDown(axis, emin,dispatcher,true);
+
+               if (dmax > 0)
+                       sortMaxUp(axis, emax,dispatcher,true);
+
+               // shrink (only removes overlaps)
+               if (dmin > 0)
+                       sortMinUp(axis, emin,dispatcher,true);
+
+               if (dmax < 0)
+                       sortMaxDown(axis, emax,dispatcher,true);
+
+#ifdef DEBUG_BROADPHASE
+       debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+       }
+
+       
+}
+
+
+
+
+// sorting a min edge downwards can only ever *add* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
+{
+
+       Edge* pEdge = m_pEdges[axis] + edge;
+       Edge* pPrev = pEdge - 1;
+       Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+       while (pEdge->m_pos < pPrev->m_pos)
+       {
+               Handle* pHandlePrev = getHandle(pPrev->m_handle);
+
+               if (pPrev->IsMax())
+               {
+                       // if previous edge is a maximum check the bounds and add an overlap if necessary
+                       if (updateOverlaps && testOverlap(axis,pHandleEdge, pHandlePrev))
+                       {
+                               m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev);
+                               if (m_userPairCallback)
+                                       m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev);
+
+                               //AddOverlap(pEdge->m_handle, pPrev->m_handle);
+
+                       }
+
+                       // update edge reference in other handle
+                       pHandlePrev->m_maxEdges[axis]++;
+               }
+               else
+                       pHandlePrev->m_minEdges[axis]++;
+
+               pHandleEdge->m_minEdges[axis]--;
+
+               // swap the edges
+               Edge swap = *pEdge;
+               *pEdge = *pPrev;
+               *pPrev = swap;
+
+               // decrement
+               pEdge--;
+               pPrev--;
+       }
+
+#ifdef DEBUG_BROADPHASE
+       debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
+}
+
+// sorting a min edge upwards can only ever *remove* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
+{
+       Edge* pEdge = m_pEdges[axis] + edge;
+       Edge* pNext = pEdge + 1;
+       Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+       while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
+       {
+               Handle* pHandleNext = getHandle(pNext->m_handle);
+
+               if (pNext->IsMax())
+               {
+
+                       // 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);
+
+                               m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); 
+                               if (m_userPairCallback)
+                                       m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
+                               
+                       }
+
+
+                       // update edge reference in other handle
+                       pHandleNext->m_maxEdges[axis]--;
+               }
+               else
+                       pHandleNext->m_minEdges[axis]--;
+
+               pHandleEdge->m_minEdges[axis]++;
+
+               // swap the edges
+               Edge swap = *pEdge;
+               *pEdge = *pNext;
+               *pNext = swap;
+
+               // increment
+               pEdge++;
+               pNext++;
+       }
+
+
+}
+
+// sorting a max edge downwards can only ever *remove* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
+{
+
+       Edge* pEdge = m_pEdges[axis] + edge;
+       Edge* pPrev = pEdge - 1;
+       Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+       while (pEdge->m_pos < pPrev->m_pos)
+       {
+               Handle* pHandlePrev = getHandle(pPrev->m_handle);
+
+               if (!pPrev->IsMax())
+               {
+                       // 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);
+                               m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
+                               if (m_userPairCallback)
+                                       m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
+                       
+
+
+                       }
+
+                       // update edge reference in other handle
+                       pHandlePrev->m_minEdges[axis]++;;
+               }
+               else
+                       pHandlePrev->m_maxEdges[axis]++;
+
+               pHandleEdge->m_maxEdges[axis]--;
+
+               // swap the edges
+               Edge swap = *pEdge;
+               *pEdge = *pPrev;
+               *pPrev = swap;
+
+               // decrement
+               pEdge--;
+               pPrev--;
+       }
+
+       
+#ifdef DEBUG_BROADPHASE
+       debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
+}
+
+// sorting a max edge upwards can only ever *add* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
+{
+       Edge* pEdge = m_pEdges[axis] + edge;
+       Edge* pNext = pEdge + 1;
+       Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+       while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
+       {
+               Handle* pHandleNext = getHandle(pNext->m_handle);
+
+               if (!pNext->IsMax())
+               {
+                       // if next edge is a minimum check the bounds and add an overlap if necessary
+                       if (updateOverlaps && testOverlap(axis, pHandleEdge, pHandleNext))
+                       {
+                               Handle* handle0 = getHandle(pEdge->m_handle);
+                               Handle* handle1 = getHandle(pNext->m_handle);
+                               m_pairCache->addOverlappingPair(handle0,handle1);
+                               if (m_userPairCallback)
+                                       m_userPairCallback->addOverlappingPair(handle0,handle1);
+                       }
+
+                       // update edge reference in other handle
+                       pHandleNext->m_minEdges[axis]--;
+               }
+               else
+                       pHandleNext->m_maxEdges[axis]--;
+
+               pHandleEdge->m_maxEdges[axis]++;
+
+               // swap the edges
+               Edge swap = *pEdge;
+               *pEdge = *pNext;
+               *pNext = swap;
+
+               // increment
+               pEdge++;
+               pNext++;
+       }
+       
+}
+
+
+
+////////////////////////////////////////////////////////////////////
+
+
+/// The 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 16 bit integer coordinates instead of floats.
+/// For large worlds and many objects, use bt32BitAxisSweep3 or btDbvtBroadphase instead. bt32BitAxisSweep3 has higher precision and allows more then 16384 objects at the cost of more memory and bit of performance.
+class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int>
+{
+public:
+
+       btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0);
+
+};
+
+/// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune.
+/// This comes at the cost of more memory per handle, and a bit slower performance.
+/// It uses arrays rather then lists for storage of the 3 axis.
+class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int>
+{
+public:
+
+       bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0);
 
 };
 
index b6ace03c07af5c451e2fdb95d9af8d96b4508453..200ac365329edc717c3822ce2a43ec77c2c38806 100644 (file)
@@ -20,20 +20,34 @@ subject to the following restrictions:
 
 struct btDispatcherInfo;
 class btDispatcher;
-struct btBroadphaseProxy;
-#include "../../LinearMath/btVector3.h"
+#include "btBroadphaseProxy.h"
+class btOverlappingPairCache;
 
-///BroadphaseInterface for aabb-overlapping object pairs
+#include "LinearMath/btVector3.h"
+
+///The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
+///Some implementations for this broadphase interface include btAxisSweep3, bt32BitAxisSweep3 and btDbvtBroadphase.
+///The actual overlapping pair management, storage, adding and removing of pairs is dealt by the btOverlappingPairCache class.
 class btBroadphaseInterface
 {
 public:
        virtual ~btBroadphaseInterface() {}
 
-       virtual btBroadphaseProxy*      createProxy(  const btVector3& min,  const btVector3& max,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =0;
-       virtual void    destroyProxy(btBroadphaseProxy* proxy)=0;
-       virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)=0;
-       virtual void    cleanProxyFromPairs(btBroadphaseProxy* proxy)=0;
+       virtual btBroadphaseProxy*      createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) =0;
+       virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
+       virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
        
+       ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+       virtual void    calculateOverlappingPairs(btDispatcher* dispatcher)=0;
+
+       virtual btOverlappingPairCache* getOverlappingPairCache()=0;
+       virtual const btOverlappingPairCache*   getOverlappingPairCache() const =0;
+
+       ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+       ///will add some transform later
+       virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0;
+
+       virtual void    printStats() = 0;
 
 };
 
index 40d9748ffa91e941ad6212172a60d27001bb596f..e0bb67f8521e5714ef40a5823960bfd7dbf30164 100644 (file)
@@ -16,7 +16,8 @@ subject to the following restrictions:
 #ifndef BROADPHASE_PROXY_H
 #define BROADPHASE_PROXY_H
 
-#include "../../LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
+#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
+#include "LinearMath/btAlignedAllocator.h"
 
 
 /// btDispatcher uses these types
@@ -38,6 +39,7 @@ IMPLICIT_CONVEX_SHAPES_START_HERE,
        CONE_SHAPE_PROXYTYPE,
        CONVEX_SHAPE_PROXYTYPE,
        CYLINDER_SHAPE_PROXYTYPE,
+       UNIFORM_SCALING_SHAPE_PROXYTYPE,
        MINKOWSKI_SUM_SHAPE_PROXYTYPE,
        MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE,
 //concave shapes
@@ -50,6 +52,8 @@ CONCAVE_SHAPES_START_HERE,
        TERRAIN_SHAPE_PROXYTYPE,
 ///Used for GIMPACT Trimesh integration
        GIMPACT_SHAPE_PROXYTYPE,
+///Multimaterial mesh
+    MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE,
        
        EMPTY_SHAPE_PROXYTYPE,
        STATIC_PLANE_PROXYTYPE,
@@ -57,13 +61,18 @@ CONCAVE_SHAPES_END_HERE,
 
        COMPOUND_SHAPE_PROXYTYPE,
 
+       SOFTBODY_SHAPE_PROXYTYPE,
+
        MAX_BROADPHASE_COLLISION_TYPES
 };
 
 
-///btBroadphaseProxy
-struct btBroadphaseProxy
+///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases. 
+///It stores collision shape type information, collision filter information and a client object, typically a btCollisionObject or btRigidBody.
+ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy
 {
+
+BT_DECLARE_ALIGNED_ALLOCATOR();
        
        ///optional filtering to cull potential collisions
        enum CollisionFilterGroups
@@ -73,44 +82,60 @@ struct btBroadphaseProxy
                KinematicFilter = 4,
                DebrisFilter = 8,
                        SensorTrigger = 16,
-               AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
+               AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
        };
 
        //Usually the client btCollisionObject or Rigidbody class
        void*   m_clientObject;
+
        short int m_collisionFilterGroup;
        short int m_collisionFilterMask;
 
+       void*   m_multiSapParentProxy;          
+
+
+       int                     m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+
+       SIMD_FORCE_INLINE int getUid() const
+       {
+               return m_uniqueId;
+       }
+
        //used for memory pools
-       btBroadphaseProxy() :m_clientObject(0){}
+       btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0)
+       {
+       }
 
-       btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask)
+       btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
                :m_clientObject(userPtr),
                m_collisionFilterGroup(collisionFilterGroup),
                m_collisionFilterMask(collisionFilterMask)
        {
+               m_multiSapParentProxy = multiSapParentProxy;
        }
 
-       static inline bool isPolyhedral(int proxyType)
+       
+
+       static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType)
        {
                return (proxyType  < IMPLICIT_CONVEX_SHAPES_START_HERE);
        }
 
-       static inline bool      isConvex(int proxyType)
+       static SIMD_FORCE_INLINE bool   isConvex(int proxyType)
        {
                return (proxyType < CONCAVE_SHAPES_START_HERE);
        }
 
-       static inline bool      isConcave(int proxyType)
+       static SIMD_FORCE_INLINE bool   isConcave(int proxyType)
        {
                return ((proxyType > CONCAVE_SHAPES_START_HERE) &&
                        (proxyType < CONCAVE_SHAPES_END_HERE));
        }
-       static inline bool      isCompound(int proxyType)
+       static SIMD_FORCE_INLINE bool   isCompound(int proxyType)
        {
                return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
        }
-       static inline bool isInfinite(int proxyType)
+       static SIMD_FORCE_INLINE bool isInfinite(int proxyType)
        {
                return (proxyType == STATIC_PLANE_PROXYTYPE);
        }
@@ -124,8 +149,9 @@ struct btBroadphaseProxy;
 
 
 
-/// contains a pair of aabb-overlapping objects
-struct btBroadphasePair
+///The btBroadphasePair class contains a pair of aabb-overlapping objects.
+///A btDispatcher can search a btCollisionAlgorithm that performs exact/narrowphase collision detection on the actual collision shapes.
+ATTRIBUTE_ALIGNED16(struct) btBroadphasePair
 {
        btBroadphasePair ()
                :
@@ -136,6 +162,8 @@ struct btBroadphasePair
        {
        }
 
+BT_DECLARE_ALIGNED_ALLOCATOR();
+
        btBroadphasePair(const btBroadphasePair& other)
                :               m_pProxy0(other.m_pProxy0),
                                m_pProxy1(other.m_pProxy1),
@@ -181,6 +209,7 @@ SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePa
 */
 
 
+
 class btBroadphasePairSortPredicate
 {
        public:
index 2ad0c86d8a2d7551e6ac25353934354d85240925..c95d1be0f2ce667f123d3b245ab57ca55591ffb8 100644 (file)
@@ -18,6 +18,6 @@ subject to the following restrictions:
 
 btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
 {
-       m_dispatcher = ci.m_dispatcher;
+       m_dispatcher = ci.m_dispatcher1;
 }
 
index 55cec386a7bf19d41b9e7214e56982b2747202d8..1618ad9fdd371f49304d267b1d596aaa0001e6c0 100644 (file)
@@ -16,7 +16,8 @@ subject to the following restrictions:
 #ifndef COLLISION_ALGORITHM_H
 #define COLLISION_ALGORITHM_H
 
-#include "../../LinearMath/btScalar.h"
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
 
 struct btBroadphaseProxy;
 class btDispatcher;
@@ -25,21 +26,22 @@ class btCollisionObject;
 struct btDispatcherInfo;
 class  btPersistentManifold;
 
+typedef btAlignedObjectArray<btPersistentManifold*>    btManifoldArray;
 
 struct btCollisionAlgorithmConstructionInfo
 {
        btCollisionAlgorithmConstructionInfo()
-               :m_dispatcher(0),
+               :m_dispatcher1(0),
                m_manifold(0)
        {
        }
        btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp)
-               :m_dispatcher(dispatcher)
+               :m_dispatcher1(dispatcher)
        {
                (void)temp;
        }
 
-       btDispatcher*   m_dispatcher;
+       btDispatcher*   m_dispatcher1;
        btPersistentManifold*   m_manifold;
 
        int     getDispatcherId();
@@ -71,6 +73,7 @@ public:
 
        virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
 
+       virtual void    getAllContactManifolds(btManifoldArray& manifoldArray) = 0;
 };
 
 
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp
new file mode 100644 (file)
index 0000000..fade711
--- /dev/null
@@ -0,0 +1,1320 @@
+/*
+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.
+*/
+///btDbvt implementation by Nathanael Presson
+
+#include "btDbvt.h"
+
+//
+typedef btAlignedObjectArray<btDbvtNode*>                      tNodeArray;
+typedef btAlignedObjectArray<const btDbvtNode*>        tConstNodeArray;
+
+//
+struct btDbvtNodeEnumerator : btDbvt::ICollide
+{
+tConstNodeArray        nodes;
+void Process(const btDbvtNode* n) { nodes.push_back(n); }
+};
+
+//
+static DBVT_INLINE int                 indexof(const btDbvtNode* node)
+{
+return(node->parent->childs[1]==node);
+}
+
+//
+static DBVT_INLINE btDbvtVolume        merge(  const btDbvtVolume& a,
+                                                                               const btDbvtVolume& b)
+{
+#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
+DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)];
+btDbvtVolume&  res=*(btDbvtVolume*)locals;
+#else
+btDbvtVolume   res;
+#endif
+Merge(a,b,res);
+return(res);
+}
+
+// volume+edge lengths
+static DBVT_INLINE btScalar            size(const btDbvtVolume& a)
+{
+const btVector3        edges=a.Lengths();
+return(        edges.x()*edges.y()*edges.z()+
+               edges.x()+edges.y()+edges.z());
+}
+
+//
+static void                                            getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth)
+{
+if(node->isinternal())
+       {
+       getmaxdepth(node->childs[0],depth+1,maxdepth);
+       getmaxdepth(node->childs[0],depth+1,maxdepth);
+       } else maxdepth=btMax(maxdepth,depth);
+}
+
+//
+static DBVT_INLINE void                        deletenode(     btDbvt* pdbvt,
+                                                                                       btDbvtNode* node)
+{
+btAlignedFree(pdbvt->m_free);
+pdbvt->m_free=node;
+}
+       
+//
+static void                                            recursedeletenode(      btDbvt* pdbvt,
+                                                                                                       btDbvtNode* node)
+{
+if(!node->isleaf())
+       {
+       recursedeletenode(pdbvt,node->childs[0]);
+       recursedeletenode(pdbvt,node->childs[1]);
+       }
+if(node==pdbvt->m_root) pdbvt->m_root=0;
+deletenode(pdbvt,node);
+}
+
+//
+static DBVT_INLINE btDbvtNode* createnode(     btDbvt* pdbvt,
+                                                                                       btDbvtNode* parent,
+                                                                                       void* data)
+{
+btDbvtNode*    node;
+if(pdbvt->m_free)
+       { node=pdbvt->m_free;pdbvt->m_free=0; }
+       else
+       { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); }
+node->parent   =       parent;
+node->data             =       data;
+node->childs[1]        =       0;
+return(node);
+}
+
+//
+static DBVT_INLINE btDbvtNode* createnode(     btDbvt* pdbvt,
+                                                                                       btDbvtNode* parent,
+                                                                                       const btDbvtVolume& volume,
+                                                                                       void* data)
+{
+btDbvtNode*    node=createnode(pdbvt,parent,data);
+node->volume=volume;
+return(node);
+}
+
+//
+static DBVT_INLINE btDbvtNode* createnode(     btDbvt* pdbvt,
+                                                                                       btDbvtNode* parent,
+                                                                                       const btDbvtVolume& volume0,
+                                                                                       const btDbvtVolume& volume1,
+                                                                                       void* data)
+{
+btDbvtNode*    node=createnode(pdbvt,parent,data);
+Merge(volume0,volume1,node->volume);
+return(node);
+}
+
+//
+static void                                            insertleaf(     btDbvt* pdbvt,
+                                                                                       btDbvtNode* root,
+                                                                                       btDbvtNode* leaf)
+{
+if(!pdbvt->m_root)
+       {
+       pdbvt->m_root   =       leaf;
+       leaf->parent    =       0;
+       }
+       else
+       {
+       if(!root->isleaf())
+               {
+               do      {
+                       root=root->childs[Select(       leaf->volume,
+                                                                               root->childs[0]->volume,
+                                                                               root->childs[1]->volume)];
+                       } while(!root->isleaf());
+               }
+       btDbvtNode*     prev=root->parent;
+       btDbvtNode*     node=createnode(pdbvt,prev,leaf->volume,root->volume,0);
+       if(prev)
+               {
+               prev->childs[indexof(root)]     =       node;
+               node->childs[0]                         =       root;root->parent=node;
+               node->childs[1]                         =       leaf;leaf->parent=node;
+               do      {
+                       if(!prev->volume.Contain(node->volume))
+                               Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
+                               else
+                               break;
+                       node=prev;
+                       } while(0!=(prev=node->parent));
+               }
+               else
+               {
+               node->childs[0] =       root;root->parent=node;
+               node->childs[1] =       leaf;leaf->parent=node;
+               pdbvt->m_root   =       node;
+               }
+       }
+}
+       
+//
+static btDbvtNode*                             removeleaf(     btDbvt* pdbvt,
+                                                                                       btDbvtNode* leaf)
+{
+if(leaf==pdbvt->m_root)
+       {
+       pdbvt->m_root=0;
+       return(0);
+       }
+       else
+       {
+       btDbvtNode*     parent=leaf->parent;
+       btDbvtNode*     prev=parent->parent;
+       btDbvtNode*     sibling=parent->childs[1-indexof(leaf)];                        
+       if(prev)
+               {
+               prev->childs[indexof(parent)]=sibling;
+               sibling->parent=prev;
+               deletenode(pdbvt,parent);
+               while(prev)
+                       {
+                       const btDbvtVolume      pb=prev->volume;
+                       Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
+                       if(NotEqual(pb,prev->volume))
+                               {
+                               prev=prev->parent;
+                               } else break;
+                       }
+               return(prev?prev:pdbvt->m_root);
+               }
+               else
+               {                                                               
+               pdbvt->m_root=sibling;
+               sibling->parent=0;
+               deletenode(pdbvt,parent);
+               return(pdbvt->m_root);
+               }                       
+       }
+}
+
+//
+static void                                            fetchleaves(btDbvt* pdbvt,
+                                                                                       btDbvtNode* root,
+                                                                                       tNodeArray& leaves,
+                                                                                       int depth=-1)
+{
+if(root->isinternal()&&depth)
+       {
+       fetchleaves(pdbvt,root->childs[0],leaves,depth-1);
+       fetchleaves(pdbvt,root->childs[1],leaves,depth-1);
+       deletenode(pdbvt,root);
+       }
+       else
+       {
+       leaves.push_back(root);
+       }
+}
+
+//
+static void                                            split(  const tNodeArray& leaves,
+                                                                               tNodeArray& left,
+                                                                               tNodeArray& right,
+                                                                               const btVector3& org,
+                                                                               const btVector3& axis)
+{
+left.resize(0);
+right.resize(0);
+for(int i=0,ni=leaves.size();i<ni;++i)
+       {
+       if(dot(axis,leaves[i]->volume.Center()-org)<0)
+               left.push_back(leaves[i]);
+               else
+               right.push_back(leaves[i]);
+       }
+}
+
+//
+static btDbvtVolume                            bounds( const tNodeArray& leaves)
+{
+#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
+DBVT_ALIGN char        locals[sizeof(btDbvtVolume)];
+btDbvtVolume&  volume=*(btDbvtVolume*)locals;
+volume=leaves[0]->volume;
+#else
+btDbvtVolume volume=leaves[0]->volume;
+#endif
+for(int i=1,ni=leaves.size();i<ni;++i)
+       {
+       Merge(volume,leaves[i]->volume,volume);
+       }
+return(volume);
+}
+
+//
+static void                                            bottomup(       btDbvt* pdbvt,
+                                                                                       tNodeArray& leaves)
+{
+while(leaves.size()>1)
+       {
+       btScalar        minsize=SIMD_INFINITY;
+       int                     minidx[2]={-1,-1};
+       for(int i=0;i<leaves.size();++i)
+               {
+               for(int j=i+1;j<leaves.size();++j)
+                       {
+                       const btScalar  sz=size(merge(leaves[i]->volume,leaves[j]->volume));
+                       if(sz<minsize)
+                               {
+                               minsize         =       sz;
+                               minidx[0]       =       i;
+                               minidx[1]       =       j;
+                               }
+                       }
+               }
+       btDbvtNode*     n[]     =       {leaves[minidx[0]],leaves[minidx[1]]};
+       btDbvtNode*     p       =       createnode(pdbvt,0,n[0]->volume,n[1]->volume,0);
+       p->childs[0]            =       n[0];
+       p->childs[1]            =       n[1];
+       n[0]->parent            =       p;
+       n[1]->parent            =       p;
+       leaves[minidx[0]]       =       p;
+       leaves.swap(minidx[1],leaves.size()-1);
+       leaves.pop_back();
+       }
+}
+
+//
+static btDbvtNode*                     topdown(btDbvt* pdbvt,
+                                                                       tNodeArray& leaves,
+                                                                       int bu_treshold)
+{
+static const btVector3 axis[]={btVector3(1,0,0),
+                                                               btVector3(0,1,0),
+                                                               btVector3(0,0,1)};
+if(leaves.size()>1)
+       {
+       if(leaves.size()>bu_treshold)
+               {
+               const btDbvtVolume      vol=bounds(leaves);
+               const btVector3                 org=vol.Center();
+               tNodeArray                              sets[2];
+               int                                             bestaxis=-1;
+               int                                             bestmidp=leaves.size();
+               int                                             splitcount[3][2]={{0,0},{0,0},{0,0}};
+               int i;
+               for( i=0;i<leaves.size();++i)
+                       {
+                       const btVector3 x=leaves[i]->volume.Center()-org;
+                       for(int j=0;j<3;++j)
+                               {
+                               ++splitcount[j][dot(x,axis[j])>0?1:0];
+                               }
+                       }
+               for( i=0;i<3;++i)
+                       {
+                       if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
+                               {
+                               const int       midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1]));
+                               if(midp<bestmidp)
+                                       {
+                                       bestaxis=i;
+                                       bestmidp=midp;
+                                       }
+                               }
+                       }
+               if(bestaxis>=0)
+                       {
+                       sets[0].reserve(splitcount[bestaxis][0]);
+                       sets[1].reserve(splitcount[bestaxis][1]);
+                       split(leaves,sets[0],sets[1],org,axis[bestaxis]);
+                       }
+                       else
+                       {
+                       sets[0].reserve(leaves.size()/2+1);
+                       sets[1].reserve(leaves.size()/2);
+                       for(int i=0,ni=leaves.size();i<ni;++i)
+                               {
+                               sets[i&1].push_back(leaves[i]);
+                               }
+                       }
+               btDbvtNode*     node=createnode(pdbvt,0,vol,0);
+               node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
+               node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
+               node->childs[0]->parent=node;
+               node->childs[1]->parent=node;
+               return(node);
+               }
+               else
+               {
+               bottomup(pdbvt,leaves);
+               return(leaves[0]);
+               }
+       }
+return(leaves[0]);
+}
+
+//
+static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r)
+{
+btDbvtNode*    p=n->parent;
+btAssert(n->isinternal());
+if(p>n)
+       {
+       const int               i=indexof(n);
+       const int               j=1-i;
+       btDbvtNode*     s=p->childs[j];
+       btDbvtNode*     q=p->parent;
+       btAssert(n==p->childs[i]);
+       if(q) q->childs[indexof(p)]=n; else r=n;
+       s->parent=n;
+       p->parent=n;
+       n->parent=q;
+       p->childs[0]=n->childs[0];
+       p->childs[1]=n->childs[1];
+       n->childs[0]->parent=p;
+       n->childs[1]->parent=p;
+       n->childs[i]=p;
+       n->childs[j]=s;
+       btSwap(p->volume,n->volume);
+       return(p);
+       }
+return(n);
+}
+
+//
+static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count)
+{
+while(n&&(count--)) n=n->parent;
+return(n);
+}
+
+//
+// Api
+//
+
+//
+                               btDbvt::btDbvt()
+{
+m_root         =       0;
+m_free         =       0;
+m_lkhd         =       -1;
+m_leaves       =       0;
+m_opath                =       0;
+}
+
+//
+                               btDbvt::~btDbvt()
+{
+clear();
+}
+
+//
+void                   btDbvt::clear()
+{
+if(m_root)     recursedeletenode(this,m_root);
+btAlignedFree(m_free);
+m_free=0;
+}
+
+//
+void                   btDbvt::optimizeBottomUp()
+{
+if(m_root)
+       {
+       tNodeArray leaves;
+       leaves.reserve(m_leaves);
+       fetchleaves(this,m_root,leaves);
+       bottomup(this,leaves);
+       m_root=leaves[0];
+       }
+}
+
+//
+void                   btDbvt::optimizeTopDown(int bu_treshold)
+{
+if(m_root)
+       {
+       tNodeArray      leaves;
+       leaves.reserve(m_leaves);
+       fetchleaves(this,m_root,leaves);
+       m_root=topdown(this,leaves,bu_treshold);
+       }
+}
+
+//
+void                   btDbvt::optimizeIncremental(int passes)
+{
+if(passes<0) passes=m_leaves;
+if(m_root&&(passes>0))
+       {
+       do      {
+               btDbvtNode*             node=m_root;
+               unsigned        bit=0;
+               while(node->isinternal())
+                       {
+                       node=sort(node,m_root)->childs[(m_opath>>bit)&1];
+                       bit=(bit+1)&(sizeof(unsigned)*8-1);
+                       }
+               update(node);
+               ++m_opath;
+               } while(--passes);
+       }
+}
+
+//
+btDbvtNode*    btDbvt::insert(const btDbvtVolume& volume,void* data)
+{
+btDbvtNode*    leaf=createnode(this,0,volume,data);
+insertleaf(this,m_root,leaf);
+++m_leaves;
+return(leaf);
+}
+
+//
+void                   btDbvt::update(btDbvtNode* leaf,int lookahead)
+{
+btDbvtNode*    root=removeleaf(this,leaf);
+if(root)
+       {
+       if(lookahead>=0)
+               {
+               for(int i=0;(i<lookahead)&&root->parent;++i)
+                       {
+                       root=root->parent;
+                       }
+               } else root=m_root;
+       }
+insertleaf(this,root,leaf);
+}
+
+//
+void                   btDbvt::update(btDbvtNode* leaf,const btDbvtVolume& volume)
+{
+btDbvtNode*    root=removeleaf(this,leaf);
+if(root)
+       {
+       if(m_lkhd>=0)
+               {
+               for(int i=0;(i<m_lkhd)&&root->parent;++i)
+                       {
+                       root=root->parent;
+                       }
+               } else root=m_root;
+       }
+leaf->volume=volume;
+insertleaf(this,root,leaf);
+}
+
+//
+bool                   btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin)
+{
+if(leaf->volume.Contain(volume)) return(false);
+volume.Expand(btVector3(margin,margin,margin));
+volume.SignedExpand(velocity);
+update(leaf,volume);
+return(true);
+}
+
+//
+bool                   btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity)
+{
+if(leaf->volume.Contain(volume)) return(false);
+volume.SignedExpand(velocity);
+update(leaf,volume);
+return(true);
+}
+
+//
+bool                   btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin)
+{
+if(leaf->volume.Contain(volume)) return(false);
+volume.Expand(btVector3(margin,margin,margin));
+update(leaf,volume);
+return(true);
+}
+
+//
+void                   btDbvt::remove(btDbvtNode* leaf)
+{
+removeleaf(this,leaf);
+deletenode(this,leaf);
+--m_leaves;
+}
+
+//
+void                   btDbvt::write(IWriter* iwriter) const
+{
+btDbvtNodeEnumerator   nodes;
+nodes.nodes.reserve(m_leaves*2);
+enumNodes(m_root,nodes);
+iwriter->Prepare(m_root,nodes.nodes.size());
+for(int i=0;i<nodes.nodes.size();++i)
+       {
+       const btDbvtNode* n=nodes.nodes[i];
+       int                     p=-1;
+       if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
+       if(n->isinternal())
+               {
+               const int       c0=nodes.nodes.findLinearSearch(n->childs[0]);
+               const int       c1=nodes.nodes.findLinearSearch(n->childs[1]);
+               iwriter->WriteNode(n,i,p,c0,c1);
+               }
+               else
+               {
+               iwriter->WriteLeaf(n,i,p);
+               }       
+       }
+}
+
+//
+void                   btDbvt::clone(btDbvt& dest,IClone* iclone) const
+{
+dest.clear();
+if(m_root!=0)
+       {       
+       btAlignedObjectArray<sStkCLN>   stack;
+       stack.reserve(m_leaves);
+       stack.push_back(sStkCLN(m_root,0));
+       do      {
+               const int               i=stack.size()-1;
+               const sStkCLN   e=stack[i];
+               btDbvtNode*                     n=createnode(&dest,e.parent,e.node->volume,e.node->data);
+               stack.pop_back();
+               if(e.parent!=0)
+                       e.parent->childs[i&1]=n;
+                       else
+                       dest.m_root=n;
+               if(e.node->isinternal())
+                       {
+                       stack.push_back(sStkCLN(e.node->childs[0],n));
+                       stack.push_back(sStkCLN(e.node->childs[1],n));
+                       }
+                       else
+                       {
+                       iclone->CloneLeaf(n);
+                       }
+               } while(stack.size()>0);
+       }
+}
+
+//
+int                            btDbvt::maxdepth(const btDbvtNode* node)
+{
+int    depth=0;
+if(node) getmaxdepth(node,1,depth);
+return(depth);
+}
+
+//
+int                            btDbvt::countLeaves(const btDbvtNode* node)
+{
+if(node->isinternal())
+       return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
+       else
+       return(1);
+}
+
+//
+void                   btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves)
+{
+if(node->isinternal())
+       {
+       extractLeaves(node->childs[0],leaves);
+       extractLeaves(node->childs[1],leaves);
+       }
+       else
+       {
+       leaves.push_back(node);
+       }       
+}
+
+//
+#if DBVT_ENABLE_BENCHMARK
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "LinearMath/btQuickProf.h"
+
+/*
+q6600,2.4ghz
+
+/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32"
+/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch"
+/Fo"..\..\out\release8\build\libbulletcollision\\"
+/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb"
+/W3 /nologo /c /Wp64 /Zi /errorReport:prompt
+
+Benchmarking dbvt...
+        World scale: 100.000000
+        Extents base: 1.000000
+        Extents range: 4.000000
+        Leaves: 8192
+        sizeof(btDbvtVolume): 32 bytes
+        sizeof(btDbvtNode):   44 bytes
+[1] btDbvtVolume intersections: 3537 ms (0%)
+[2] btDbvtVolume merges: 1945 ms (0%)
+[3] btDbvt::collideTT: 6646 ms (0%)
+[4] btDbvt::collideTT self: 3389 ms (0%)
+[5] btDbvt::collideTT xform: 7505 ms (0%)
+[6] btDbvt::collideTT xform,self: 7480 ms (0%)
+[7] btDbvt::collideRAY: 6307 ms (0%),(332511 r/s)
+[8] insert/remove: 2105 ms (-3%),(996271 ir/s)
+[9] updates (teleport): 1943 ms (0%),(1079337 u/s)
+[10] updates (jitter): 1301 ms (0%),(1611953 u/s)
+[11] optimize (incremental): 2510 ms (0%),(1671000 o/s)
+[12] btDbvtVolume notequal: 3677 ms (0%)
+[13] culling(OCL+fullsort): 2231 ms (0%),(458 t/s)
+[14] culling(OCL+qsort): 3500 ms (0%),(2340 t/s)
+[15] culling(KDOP+qsort): 1151 ms (0%),(7117 t/s)
+[16] insert/remove batch(256): 5138 ms (0%),(816330 bir/s)
+[17] btDbvtVolume proximity: 2842 ms (0%)
+[18] btDbvtVolume select: 3390 ms (0%)
+*/
+
+struct btDbvtBenchmark
+{
+struct NilPolicy : btDbvt::ICollide
+       {
+       NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true)             {}
+       void    Process(const btDbvtNode*,const btDbvtNode*)                            { ++m_pcount; }
+       void    Process(const btDbvtNode*)                                                                      { ++m_pcount; }
+       void    Process(const btDbvtNode*,btScalar depth)
+               {
+               ++m_pcount;
+               if(m_checksort)
+                       { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); }
+               }
+       int                     m_pcount;
+       btScalar        m_depth;
+       bool            m_checksort;
+       };
+struct P14 : btDbvt::ICollide
+       {
+       struct Node
+               {
+               const btDbvtNode*       leaf;
+               btScalar                        depth;
+               };
+       void Process(const btDbvtNode* leaf,btScalar depth)
+               {
+               Node    n;
+               n.leaf  =       leaf;
+               n.depth =       depth;
+               }
+       static int sortfnc(const Node& a,const Node& b)
+               {
+               if(a.depth<b.depth) return(+1);
+               if(a.depth>b.depth) return(-1);
+               return(0);
+               }
+       btAlignedObjectArray<Node>              m_nodes;
+       };
+struct P15 : btDbvt::ICollide
+       {
+       struct Node
+               {
+               const btDbvtNode*       leaf;
+               btScalar                        depth;
+               };
+       void Process(const btDbvtNode* leaf)
+               {
+               Node    n;
+               n.leaf  =       leaf;
+               n.depth =       dot(leaf->volume.Center(),m_axis);
+               }
+       static int sortfnc(const Node& a,const Node& b)
+               {
+               if(a.depth<b.depth) return(+1);
+               if(a.depth>b.depth) return(-1);
+               return(0);
+               }
+       btAlignedObjectArray<Node>              m_nodes;
+       btVector3                                               m_axis;
+       };
+static btScalar                        RandUnit()
+       {
+       return(rand()/(btScalar)RAND_MAX);
+       }
+static btVector3               RandVector3()
+       {
+       return(btVector3(RandUnit(),RandUnit(),RandUnit()));
+       }
+static btVector3               RandVector3(btScalar cs)
+       {
+       return(RandVector3()*cs-btVector3(cs,cs,cs)/2);
+       }
+static btDbvtVolume    RandVolume(btScalar cs,btScalar eb,btScalar es)
+       {
+       return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es));
+       }
+static btTransform             RandTransform(btScalar cs)
+       {
+       btTransform     t;
+       t.setOrigin(RandVector3(cs));
+       t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized());
+       return(t);
+       }
+static void                            RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt)
+       {
+       dbvt.clear();
+       for(int i=0;i<leaves;++i)
+               {
+               dbvt.insert(RandVolume(cs,eb,es),0);
+               }
+       }
+};
+
+void                   btDbvt::benchmark()
+{
+static const btScalar  cfgVolumeCenterScale            =       100;
+static const btScalar  cfgVolumeExentsBase                     =       1;
+static const btScalar  cfgVolumeExentsScale            =       4;
+static const int               cfgLeaves                                       =       8192;
+static const bool              cfgEnable                                       =       true;
+
+//[1] btDbvtVolume intersections
+bool                                   cfgBenchmark1_Enable            =       cfgEnable;
+static const int               cfgBenchmark1_Iterations        =       8;
+static const int               cfgBenchmark1_Reference         =       3537;
+//[2] btDbvtVolume merges
+bool                                   cfgBenchmark2_Enable            =       cfgEnable;
+static const int               cfgBenchmark2_Iterations        =       4;
+static const int               cfgBenchmark2_Reference         =       1945;
+//[3] btDbvt::collideTT
+bool                                   cfgBenchmark3_Enable            =       cfgEnable;
+static const int               cfgBenchmark3_Iterations        =       512;
+static const int               cfgBenchmark3_Reference         =       6646;
+//[4] btDbvt::collideTT self
+bool                                   cfgBenchmark4_Enable            =       cfgEnable;
+static const int               cfgBenchmark4_Iterations        =       512;
+static const int               cfgBenchmark4_Reference         =       3389;
+//[5] btDbvt::collideTT xform
+bool                                   cfgBenchmark5_Enable            =       cfgEnable;
+static const int               cfgBenchmark5_Iterations        =       512;
+static const btScalar  cfgBenchmark5_OffsetScale       =       2;
+static const int               cfgBenchmark5_Reference         =       7505;
+//[6] btDbvt::collideTT xform,self
+bool                                   cfgBenchmark6_Enable            =       cfgEnable;
+static const int               cfgBenchmark6_Iterations        =       512;
+static const btScalar  cfgBenchmark6_OffsetScale       =       2;
+static const int               cfgBenchmark6_Reference         =       7480;
+//[7] btDbvt::collideRAY
+bool                                   cfgBenchmark7_Enable            =       cfgEnable;
+static const int               cfgBenchmark7_Passes            =       32;
+static const int               cfgBenchmark7_Iterations        =       65536;
+static const int               cfgBenchmark7_Reference         =       6307;
+//[8] insert/remove
+bool                                   cfgBenchmark8_Enable            =       cfgEnable;
+static const int               cfgBenchmark8_Passes            =       32;
+static const int               cfgBenchmark8_Iterations        =       65536;
+static const int               cfgBenchmark8_Reference         =       2105;
+//[9] updates (teleport)
+bool                                   cfgBenchmark9_Enable            =       cfgEnable;
+static const int               cfgBenchmark9_Passes            =       32;
+static const int               cfgBenchmark9_Iterations        =       65536;
+static const int               cfgBenchmark9_Reference         =       1943;
+//[10] updates (jitter)
+bool                                   cfgBenchmark10_Enable           =       cfgEnable;
+static const btScalar  cfgBenchmark10_Scale            =       cfgVolumeCenterScale/10000;
+static const int               cfgBenchmark10_Passes           =       32;
+static const int               cfgBenchmark10_Iterations       =       65536;
+static const int               cfgBenchmark10_Reference        =       1301;
+//[11] optimize (incremental)
+bool                                   cfgBenchmark11_Enable           =       cfgEnable;
+static const int               cfgBenchmark11_Passes           =       64;
+static const int               cfgBenchmark11_Iterations       =       65536;
+static const int               cfgBenchmark11_Reference        =       2510;
+//[12] btDbvtVolume notequal
+bool                                   cfgBenchmark12_Enable           =       cfgEnable;
+static const int               cfgBenchmark12_Iterations       =       32;
+static const int               cfgBenchmark12_Reference        =       3677;
+//[13] culling(OCL+fullsort)
+bool                                   cfgBenchmark13_Enable           =       cfgEnable;
+static const int               cfgBenchmark13_Iterations       =       1024;
+static const int               cfgBenchmark13_Reference        =       2231;
+//[14] culling(OCL+qsort)
+bool                                   cfgBenchmark14_Enable           =       cfgEnable;
+static const int               cfgBenchmark14_Iterations       =       8192;
+static const int               cfgBenchmark14_Reference        =       3500;
+//[15] culling(KDOP+qsort)
+bool                                   cfgBenchmark15_Enable           =       cfgEnable;
+static const int               cfgBenchmark15_Iterations       =       8192;
+static const int               cfgBenchmark15_Reference        =       1151;
+//[16] insert/remove batch
+bool                                   cfgBenchmark16_Enable           =       cfgEnable;
+static const int               cfgBenchmark16_BatchCount       =       256;
+static const int               cfgBenchmark16_Passes           =       16384;
+static const int               cfgBenchmark16_Reference        =       5138;
+//[17] proximity
+bool                                   cfgBenchmark17_Enable           =       cfgEnable;
+static const int               cfgBenchmark17_Iterations       =       8;
+static const int               cfgBenchmark17_Reference        =       2842;
+//[18] select
+bool                                   cfgBenchmark18_Enable           =       cfgEnable;
+static const int               cfgBenchmark18_Iterations       =       4;
+static const int               cfgBenchmark18_Reference        =       3390;
+
+btClock                                        wallclock;
+printf("Benchmarking dbvt...\r\n");
+printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
+printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
+printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
+printf("\tLeaves: %u\r\n",cfgLeaves);
+printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume));
+printf("\tsizeof(btDbvtNode):   %u bytes\r\n",sizeof(btDbvtNode));
+if(cfgBenchmark1_Enable)
+       {// Benchmark 1 
+       srand(380843);
+       btAlignedObjectArray<btDbvtVolume>      volumes;
+       btAlignedObjectArray<bool>                      results;
+       volumes.resize(cfgLeaves);
+       results.resize(cfgLeaves);
+       for(int i=0;i<cfgLeaves;++i)
+               {
+               volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+               }
+       printf("[1] btDbvtVolume intersections: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark1_Iterations;++i)
+               {
+               for(int j=0;j<cfgLeaves;++j)
+                       {
+                       for(int k=0;k<cfgLeaves;++k)
+                               {
+                               results[k]=Intersect(volumes[j],volumes[k]);
+                               }
+                       }
+               }
+       const int time=(int)wallclock.getTimeMilliseconds();
+       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time);
+       }
+if(cfgBenchmark2_Enable)
+       {// Benchmark 2 
+       srand(380843);
+       btAlignedObjectArray<btDbvtVolume>      volumes;
+       btAlignedObjectArray<btDbvtVolume>      results;
+       volumes.resize(cfgLeaves);
+       results.resize(cfgLeaves);
+       for(int i=0;i<cfgLeaves;++i)
+               {
+               volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+               }
+       printf("[2] btDbvtVolume merges: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark2_Iterations;++i)
+               {
+               for(int j=0;j<cfgLeaves;++j)
+                       {
+                       for(int k=0;k<cfgLeaves;++k)
+                               {
+                               Merge(volumes[j],volumes[k],results[k]);
+                               }
+                       }
+               }
+       const int time=(int)wallclock.getTimeMilliseconds();
+       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time);
+       }
+if(cfgBenchmark3_Enable)
+       {// Benchmark 3 
+       srand(380843);
+       btDbvt                                          dbvt[2];
+       btDbvtBenchmark::NilPolicy      policy;
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
+       dbvt[0].optimizeTopDown();
+       dbvt[1].optimizeTopDown();
+       printf("[3] btDbvt::collideTT: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark3_Iterations;++i)
+               {
+               btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy);
+               }
+       const int time=(int)wallclock.getTimeMilliseconds();
+       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time);
+       }
+if(cfgBenchmark4_Enable)
+       {// Benchmark 4
+       srand(380843);
+       btDbvt                                          dbvt;
+       btDbvtBenchmark::NilPolicy      policy;
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       printf("[4] btDbvt::collideTT self: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark4_Iterations;++i)
+               {
+               btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy);
+               }
+       const int time=(int)wallclock.getTimeMilliseconds();
+       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time);
+       }
+if(cfgBenchmark5_Enable)
+       {// Benchmark 5 
+       srand(380843);
+       btDbvt                                                          dbvt[2];
+       btAlignedObjectArray<btTransform>       transforms;
+       btDbvtBenchmark::NilPolicy                      policy;
+       transforms.resize(cfgBenchmark5_Iterations);
+       for(int i=0;i<transforms.size();++i)
+               {
+               transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
+               }
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
+       dbvt[0].optimizeTopDown();
+       dbvt[1].optimizeTopDown();
+       printf("[5] btDbvt::collideTT xform: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark5_Iterations;++i)
+               {
+               btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy);
+               }
+       const int time=(int)wallclock.getTimeMilliseconds();
+       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time);
+       }
+if(cfgBenchmark6_Enable)
+       {// Benchmark 6 
+       srand(380843);
+       btDbvt                                                          dbvt;
+       btAlignedObjectArray<btTransform>       transforms;
+       btDbvtBenchmark::NilPolicy                      policy;
+       transforms.resize(cfgBenchmark6_Iterations);
+       for(int i=0;i<transforms.size();++i)
+               {
+               transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
+               }
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       printf("[6] btDbvt::collideTT xform,self: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark6_Iterations;++i)
+               {
+               btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy);                
+               }
+       const int time=(int)wallclock.getTimeMilliseconds();
+       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time);
+       }
+if(cfgBenchmark7_Enable)
+       {// Benchmark 7 
+       srand(380843);
+       btDbvt                                                          dbvt;
+       btAlignedObjectArray<btVector3>         rayorg;
+       btAlignedObjectArray<btVector3>         raydir;
+       btDbvtBenchmark::NilPolicy                      policy;
+       rayorg.resize(cfgBenchmark7_Iterations);
+       raydir.resize(cfgBenchmark7_Iterations);
+       for(int i=0;i<rayorg.size();++i)
+               {
+               rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
+               raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
+               }
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       printf("[7] btDbvt::collideRAY: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark7_Passes;++i)
+               {
+               for(int j=0;j<cfgBenchmark7_Iterations;++j)
+                       {
+                       btDbvt::collideRAY(dbvt.m_root,rayorg[j],raydir[j],policy);
+                       }
+               }
+       const int       time=(int)wallclock.getTimeMilliseconds();
+       unsigned        rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations;
+       printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time);
+       }
+if(cfgBenchmark8_Enable)
+       {// Benchmark 8 
+       srand(380843);
+       btDbvt                                                          dbvt;
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       printf("[8] insert/remove: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark8_Passes;++i)
+               {
+               for(int j=0;j<cfgBenchmark8_Iterations;++j)
+                       {
+                       dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
+                       }
+               }
+       const int       time=(int)wallclock.getTimeMilliseconds();
+       const int       ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations;
+       printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time);
+       }
+if(cfgBenchmark9_Enable)
+       {// Benchmark 9 
+       srand(380843);
+       btDbvt                                                                          dbvt;
+       btAlignedObjectArray<const btDbvtNode*> leaves;
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       dbvt.extractLeaves(dbvt.m_root,leaves);
+       printf("[9] updates (teleport): ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark9_Passes;++i)
+               {
+               for(int j=0;j<cfgBenchmark9_Iterations;++j)
+                       {
+                       dbvt.update(const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]),
+                                               btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale));
+                       }
+               }
+       const int       time=(int)wallclock.getTimeMilliseconds();
+       const int       up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations;
+       printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time);
+       }
+if(cfgBenchmark10_Enable)
+       {// Benchmark 10        
+       srand(380843);
+       btDbvt                                                                          dbvt;
+       btAlignedObjectArray<const btDbvtNode*> leaves;
+       btAlignedObjectArray<btVector3>                         vectors;
+       vectors.resize(cfgBenchmark10_Iterations);
+       for(int i=0;i<vectors.size();++i)
+               {
+               vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale;
+               }
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       dbvt.extractLeaves(dbvt.m_root,leaves);
+       printf("[10] updates (jitter): ");
+       wallclock.reset();
+       
+       for(int i=0;i<cfgBenchmark10_Passes;++i)
+               {
+               for(int j=0;j<cfgBenchmark10_Iterations;++j)
+                       {                       
+                       const btVector3&        d=vectors[j];
+                       btDbvtNode*             l=const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]);
+                       btDbvtVolume            v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d);
+                       dbvt.update(l,v);
+                       }
+               }
+       const int       time=(int)wallclock.getTimeMilliseconds();
+       const int       up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations;
+       printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time);
+       }
+if(cfgBenchmark11_Enable)
+       {// Benchmark 11        
+       srand(380843);
+       btDbvt                                                                          dbvt;
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       printf("[11] optimize (incremental): ");
+       wallclock.reset();      
+       for(int i=0;i<cfgBenchmark11_Passes;++i)
+               {
+               dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
+               }
+       const int       time=(int)wallclock.getTimeMilliseconds();
+       const int       op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations;
+       printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000);
+       }
+if(cfgBenchmark12_Enable)
+       {// Benchmark 12        
+       srand(380843);
+       btAlignedObjectArray<btDbvtVolume>      volumes;
+       btAlignedObjectArray<bool>                              results;
+       volumes.resize(cfgLeaves);
+       results.resize(cfgLeaves);
+       for(int i=0;i<cfgLeaves;++i)
+               {
+               volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+               }
+       printf("[12] btDbvtVolume notequal: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark12_Iterations;++i)
+               {
+               for(int j=0;j<cfgLeaves;++j)
+                       {
+                       for(int k=0;k<cfgLeaves;++k)
+                               {
+                               results[k]=NotEqual(volumes[j],volumes[k]);
+                               }
+                       }
+               }
+       const int time=(int)wallclock.getTimeMilliseconds();
+       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time);
+       }
+if(cfgBenchmark13_Enable)
+       {// Benchmark 13        
+       srand(380843);
+       btDbvt                                                          dbvt;
+       btAlignedObjectArray<btVector3>         vectors;
+       btDbvtBenchmark::NilPolicy                      policy;
+       vectors.resize(cfgBenchmark13_Iterations);
+       for(int i=0;i<vectors.size();++i)
+               {
+               vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+               }
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       printf("[13] culling(OCL+fullsort): ");
+       wallclock.reset();      
+       for(int i=0;i<cfgBenchmark13_Iterations;++i)
+               {
+               static const btScalar   offset=0;
+               policy.m_depth=-SIMD_INFINITY;
+               dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy);
+               }
+       const int       time=(int)wallclock.getTimeMilliseconds();
+       const int       t=cfgBenchmark13_Iterations;
+       printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time);
+       }
+if(cfgBenchmark14_Enable)
+       {// Benchmark 14        
+       srand(380843);
+       btDbvt                                                          dbvt;
+       btAlignedObjectArray<btVector3>         vectors;
+       btDbvtBenchmark::P14                            policy;
+       vectors.resize(cfgBenchmark14_Iterations);
+       for(int i=0;i<vectors.size();++i)
+               {
+               vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+               }
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       policy.m_nodes.reserve(cfgLeaves);
+       printf("[14] culling(OCL+qsort): ");
+       wallclock.reset();      
+       for(int i=0;i<cfgBenchmark14_Iterations;++i)
+               {
+               static const btScalar   offset=0;
+               policy.m_nodes.resize(0);
+               dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false);
+               policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc);
+               }
+       const int       time=(int)wallclock.getTimeMilliseconds();
+       const int       t=cfgBenchmark14_Iterations;
+       printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time);
+       }
+if(cfgBenchmark15_Enable)
+       {// Benchmark 15        
+       srand(380843);
+       btDbvt                                                          dbvt;
+       btAlignedObjectArray<btVector3>         vectors;
+       btDbvtBenchmark::P15                            policy;
+       vectors.resize(cfgBenchmark15_Iterations);
+       for(int i=0;i<vectors.size();++i)
+               {
+               vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+               }
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       policy.m_nodes.reserve(cfgLeaves);
+       printf("[15] culling(KDOP+qsort): ");
+       wallclock.reset();      
+       for(int i=0;i<cfgBenchmark15_Iterations;++i)
+               {
+               static const btScalar   offset=0;
+               policy.m_nodes.resize(0);
+               policy.m_axis=vectors[i];
+               dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy);
+               policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc);
+               }
+       const int       time=(int)wallclock.getTimeMilliseconds();
+       const int       t=cfgBenchmark15_Iterations;
+       printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time);
+       }
+if(cfgBenchmark16_Enable)
+       {// Benchmark 16        
+       srand(380843);
+       btDbvt                                                          dbvt;
+       btAlignedObjectArray<btDbvtNode*>       batch;
+       btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+       dbvt.optimizeTopDown();
+       batch.reserve(cfgBenchmark16_BatchCount);
+       printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount);
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark16_Passes;++i)
+               {
+               for(int j=0;j<cfgBenchmark16_BatchCount;++j)
+                       {
+                       batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
+                       }
+               for(int j=0;j<cfgBenchmark16_BatchCount;++j)
+                       {
+                       dbvt.remove(batch[j]);
+                       }
+               batch.resize(0);
+               }
+       const int       time=(int)wallclock.getTimeMilliseconds();
+       const int       ir=cfgBenchmark16_Passes*cfgBenchmark16_BatchCount;
+       printf("%u ms (%i%%),(%u bir/s)\r\n",time,(time-cfgBenchmark16_Reference)*100/time,int(ir*1000.0/time));
+       }
+if(cfgBenchmark17_Enable)
+       {// Benchmark 17
+       srand(380843);
+       btAlignedObjectArray<btDbvtVolume>      volumes;
+       btAlignedObjectArray<btScalar>          results;
+       volumes.resize(cfgLeaves);
+       results.resize(cfgLeaves);
+       for(int i=0;i<cfgLeaves;++i)
+               {
+               volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+               }
+       printf("[17] btDbvtVolume proximity: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark17_Iterations;++i)
+               {
+               for(int j=0;j<cfgLeaves;++j)
+                       {
+                       for(int k=0;k<cfgLeaves;++k)
+                               {
+                               results[k]=Proximity(volumes[j],volumes[k]);
+                               }
+                       }
+               }
+       const int time=(int)wallclock.getTimeMilliseconds();
+       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time);
+       }
+if(cfgBenchmark18_Enable)
+       {// Benchmark 18
+       srand(380843);
+       btAlignedObjectArray<btDbvtVolume>      volumes;
+       btAlignedObjectArray<int>                       results;
+       btAlignedObjectArray<int>                       indices;
+       volumes.resize(cfgLeaves);
+       results.resize(cfgLeaves);
+       indices.resize(cfgLeaves);
+       for(int i=0;i<cfgLeaves;++i)
+               {
+               indices[i]=i;
+               volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+               }
+       for(int i=0;i<cfgLeaves;++i)
+               {
+               btSwap(indices[i],indices[rand()%cfgLeaves]);
+               }
+       printf("[18] btDbvtVolume select: ");
+       wallclock.reset();
+       for(int i=0;i<cfgBenchmark18_Iterations;++i)
+               {
+               for(int j=0;j<cfgLeaves;++j)
+                       {
+                       for(int k=0;k<cfgLeaves;++k)
+                               {
+                               const int idx=indices[k];
+                               results[idx]=Select(volumes[idx],volumes[j],volumes[k]);
+                               }
+                       }
+               }
+       const int time=(int)wallclock.getTimeMilliseconds();
+       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark18_Reference)*100/time);
+       }
+printf("\r\n\r\n");
+}
+#endif
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h
new file mode 100644 (file)
index 0000000..10f9462
--- /dev/null
@@ -0,0 +1,1101 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 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.
+*/
+///btDbvt implementation by Nathanael Presson
+
+#ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
+#define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+
+//
+// Compile time configuration
+//
+
+
+// Implementation profiles
+#define DBVT_IMPL_GENERIC              0       // Generic implementation       
+#define DBVT_IMPL_SSE                  1       // SSE
+
+// Template implementation of ICollide
+#ifdef WIN32_AVOID_WHEN_EMBEDDED_INSIDE_BLENDER
+       #if (defined (_MSC_VER) && _MSC_VER >= 1400)
+       #define DBVT_USE_TEMPLATE               1
+       #else
+       #define DBVT_USE_TEMPLATE               0
+#endif
+#else
+#define        DBVT_USE_TEMPLATE               0
+#endif
+
+// Using memmov for collideOCL
+#define DBVT_USE_MEMMOVE               1
+
+// Enable benchmarking code
+#define        DBVT_ENABLE_BENCHMARK   0
+
+// Inlining
+#define DBVT_INLINE                            SIMD_FORCE_INLINE
+// Align
+#ifdef WIN32
+#define DBVT_ALIGN                             __declspec(align(16))
+#else
+#define DBVT_ALIGN
+#endif
+
+// Specific methods implementation
+#ifdef WIN32_AVOID_WHEN_EMBEDDED_INSIDE_BLENDER
+#define DBVT_PROXIMITY_IMPL            DBVT_IMPL_SSE
+#define DBVT_SELECT_IMPL               DBVT_IMPL_SSE
+#define DBVT_MERGE_IMPL                        DBVT_IMPL_SSE
+#else
+#define DBVT_PROXIMITY_IMPL            DBVT_IMPL_GENERIC
+#define DBVT_SELECT_IMPL               DBVT_IMPL_GENERIC
+#define DBVT_MERGE_IMPL                        DBVT_IMPL_GENERIC
+#endif
+
+//
+// Auto config and checks
+//
+
+#if DBVT_USE_TEMPLATE
+#define        DBVT_VIRTUAL
+#define DBVT_VIRTUAL_DTOR(a)
+#define DBVT_PREFIX                                    template <typename T>
+#define DBVT_IPOLICY                           T& policy
+#define DBVT_CHECKTYPE                         static const ICollide&  typechecker=*(T*)0;
+#else
+#define        DBVT_VIRTUAL_DTOR(a)            virtual ~a() {}
+#define DBVT_VIRTUAL                           virtual
+#define DBVT_PREFIX
+#define DBVT_IPOLICY                           ICollide& policy
+#define DBVT_CHECKTYPE
+#endif
+
+#if DBVT_USE_MEMMOVE
+#ifndef __CELLOS_LV2__
+#include <memory.h>
+#endif
+#include <string.h>
+#endif
+
+#ifndef DBVT_USE_TEMPLATE
+#error "DBVT_USE_TEMPLATE undefined"
+#endif
+
+#ifndef DBVT_USE_MEMMOVE
+#error "DBVT_USE_MEMMOVE undefined"
+#endif
+
+#ifndef DBVT_ENABLE_BENCHMARK
+#error "DBVT_ENABLE_BENCHMARK undefined"
+#endif
+
+#ifndef DBVT_PROXIMITY_IMPL
+#error "DBVT_PROXIMITY_IMPL undefined"
+#endif
+
+#ifndef DBVT_SELECT_IMPL
+#error "DBVT_SELECT_IMPL undefined"
+#endif
+
+#ifndef DBVT_MERGE_IMPL
+#error "DBVT_MERGE_IMPL undefined"
+#endif
+
+//
+// Defaults volumes
+//
+
+/* btDbvtAabbMm                        */ 
+struct btDbvtAabbMm
+{
+DBVT_INLINE btVector3                  Center() const  { return((mi+mx)/2); }
+DBVT_INLINE btVector3                  Lengths() const { return(mx-mi); }
+DBVT_INLINE btVector3                  Extents() const { return((mx-mi)/2); }
+DBVT_INLINE const btVector3&   Mins() const    { return(mi); }
+DBVT_INLINE const btVector3&   Maxs() const    { return(mx); }
+static inline btDbvtAabbMm             FromCE(const btVector3& c,const btVector3& e);
+static inline btDbvtAabbMm             FromCR(const btVector3& c,btScalar r);
+static inline btDbvtAabbMm             FromMM(const btVector3& mi,const btVector3& mx);
+static inline btDbvtAabbMm             FromPoints(const btVector3* pts,int n);
+static inline btDbvtAabbMm             FromPoints(const btVector3** ppts,int n);
+DBVT_INLINE void                               Expand(const btVector3 e);
+DBVT_INLINE void                               SignedExpand(const btVector3 e);
+DBVT_INLINE bool                               Contain(const btDbvtAabbMm& a) const;
+DBVT_INLINE int                                        Classify(const btVector3& n,btScalar o,int s) const;
+DBVT_INLINE btScalar                   ProjectMinimum(const btVector3& v,unsigned signs) const;
+DBVT_INLINE friend bool                        Intersect(      const btDbvtAabbMm& a,
+                                                                                       const btDbvtAabbMm& b);
+DBVT_INLINE friend bool                        Intersect(      const btDbvtAabbMm& a,
+                                                                                       const btDbvtAabbMm& b,
+                                                                                       const btTransform& xform);
+DBVT_INLINE friend bool                        Intersect(      const btDbvtAabbMm& a,
+                                                                                       const btVector3& b);
+DBVT_INLINE friend bool                        Intersect(      const btDbvtAabbMm& a,
+                                                                                       const btVector3& org,
+                                                                                       const btVector3& invdir,
+                                                                                       const unsigned* signs);
+DBVT_INLINE friend btScalar            Proximity(      const btDbvtAabbMm& a,
+                                                                                       const btDbvtAabbMm& b);
+DBVT_INLINE friend int                 Select(         const btDbvtAabbMm& o,
+                                                                                       const btDbvtAabbMm& a,
+                                                                                       const btDbvtAabbMm& b);
+DBVT_INLINE friend void                        Merge(          const btDbvtAabbMm& a,
+                                                                                       const btDbvtAabbMm& b,
+                                                                                       btDbvtAabbMm& r);
+DBVT_INLINE friend bool                        NotEqual(       const btDbvtAabbMm& a,
+                                                                                       const btDbvtAabbMm& b);
+private:
+DBVT_INLINE void                               AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
+private:
+btVector3      mi,mx;
+};
+
+// Types       
+typedef        btDbvtAabbMm    btDbvtVolume;
+
+/* btDbvtNode                          */ 
+struct btDbvtNode
+{
+       btDbvtVolume    volume;
+       btDbvtNode*             parent;
+       bool    isleaf() const          { return(childs[1]==0); }
+       bool    isinternal() const      { return(!isleaf()); }
+       union   {
+               btDbvtNode*     childs[2];
+               void*   data;
+               };
+};
+
+///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
+///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
+///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
+struct btDbvt
+       {
+       
+       
+       /* Stack element        */ 
+       struct  sStkNN
+               {
+               const btDbvtNode*       a;
+               const btDbvtNode*       b;
+               sStkNN() {}
+               sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
+               };
+       struct  sStkNP
+               {
+               const btDbvtNode*       node;
+               int                     mask;
+               sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
+               };
+       struct  sStkNPS
+               {
+               const btDbvtNode*       node;
+               int                     mask;
+               btScalar        value;
+               sStkNPS() {}
+               sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
+               };
+       struct  sStkCLN
+               {
+               const btDbvtNode*       node;
+               btDbvtNode*             parent;
+               sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
+               };
+       // Policies/Interfaces
+                       
+       /* ICollide     */ 
+       struct  ICollide
+               {               
+               DBVT_VIRTUAL_DTOR(ICollide)
+               DBVT_VIRTUAL void       Process(const btDbvtNode*,const btDbvtNode*)            {}
+               DBVT_VIRTUAL void       Process(const btDbvtNode*)                                      {}
+               DBVT_VIRTUAL void       Process(const btDbvtNode* n,btScalar)                   { Process(n); }
+               DBVT_VIRTUAL bool       Descent(const btDbvtNode*)                                      { return(true); }
+               DBVT_VIRTUAL bool       AllLeaves(const btDbvtNode*)                                    { return(true); }
+               };
+       /* IWriter      */ 
+       struct  IWriter
+               {
+               virtual ~IWriter() {}
+               virtual void            Prepare(const btDbvtNode* root,int numnodes)=0;
+               virtual void            WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
+               virtual void            WriteLeaf(const btDbvtNode*,int index,int parent)=0;
+               };
+       /* IClone       */ 
+       struct  IClone
+               {
+               virtual ~IClone()       {}
+               virtual void            CloneLeaf(btDbvtNode*) {}
+               };
+               
+       // Constants
+       enum    {
+                       SIMPLE_STACKSIZE        =       64,
+                       DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
+                       };
+               
+       // Fields
+       btDbvtNode*                     m_root;
+       btDbvtNode*                     m_free;
+       int                             m_lkhd;
+       int                             m_leaves;
+       unsigned                m_opath;
+       // Methods
+                                       btDbvt();
+                                       ~btDbvt();
+       void                    clear();
+       bool                    empty() const { return(0==m_root); }
+       void                    optimizeBottomUp();
+       void                    optimizeTopDown(int bu_treshold=128);
+       void                    optimizeIncremental(int passes);
+       btDbvtNode*             insert(const btDbvtVolume& box,void* data);
+       void                    update(btDbvtNode* leaf,int lookahead=-1);
+       void                    update(btDbvtNode* leaf,const btDbvtVolume& volume);
+       bool                    update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin);
+       bool                    update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity);
+       bool                    update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin);   
+       void                    remove(btDbvtNode* leaf);
+       void                    write(IWriter* iwriter) const;
+       void                    clone(btDbvt& dest,IClone* iclone=0) const;
+       static int              maxdepth(const btDbvtNode* node);
+       static int              countLeaves(const btDbvtNode* node);
+       static void             extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
+       #if DBVT_ENABLE_BENCHMARK
+       static void             benchmark();
+       #else
+       static void             benchmark(){}
+       #endif
+       // DBVT_IPOLICY must support ICollide policy/interface
+       DBVT_PREFIX
+       static void             enumNodes(      const btDbvtNode* root,
+                                                               DBVT_IPOLICY);
+       DBVT_PREFIX
+       static void             enumLeaves(     const btDbvtNode* root,
+                                                               DBVT_IPOLICY);
+       DBVT_PREFIX
+       static void             collideTT(      const btDbvtNode* root0,
+                                                               const btDbvtNode* root1,
+                                                               DBVT_IPOLICY);
+       DBVT_PREFIX
+       static void             collideTT(      const btDbvtNode* root0,
+                                                               const btDbvtNode* root1,
+                                                               const btTransform& xform,
+                                                               DBVT_IPOLICY);
+       DBVT_PREFIX
+       static void             collideTT(      const btDbvtNode* root0,
+                                                               const btTransform& xform0,
+                                                               const btDbvtNode* root1,
+                                                               const btTransform& xform1,
+                                                               DBVT_IPOLICY);
+       DBVT_PREFIX
+       static void             collideTV(      const btDbvtNode* root,
+                                                               const btDbvtVolume& volume,
+                                                               DBVT_IPOLICY);
+       DBVT_PREFIX
+       static void             collideRAY(     const btDbvtNode* root,
+                                                               const btVector3& origin,
+                                                               const btVector3& direction,
+                                                               DBVT_IPOLICY);
+       DBVT_PREFIX
+       static void             collideKDOP(const btDbvtNode* root,
+                                                               const btVector3* normals,
+                                                               const btScalar* offsets,
+                                                               int count,
+                                                               DBVT_IPOLICY);
+       DBVT_PREFIX
+       static void             collideOCL(     const btDbvtNode* root,
+                                                               const btVector3* normals,
+                                                               const btScalar* offsets,
+                                                               const btVector3& sortaxis,
+                                                               int count,                                                              
+                                                               DBVT_IPOLICY,
+                                                               bool fullsort=true);
+       DBVT_PREFIX
+       static void             collideTU(      const btDbvtNode* root,
+                                                               DBVT_IPOLICY);
+       // Helpers      
+       static DBVT_INLINE int  nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
+               {
+               int     m=0;
+               while(l<h)
+                       {
+                       m=(l+h)>>1;
+                       if(a[i[m]].value>=v) l=m+1; else h=m;
+                       }
+               return(h);
+               }
+       static DBVT_INLINE int  allocate(       btAlignedObjectArray<int>& ifree,
+                                                                               btAlignedObjectArray<sStkNPS>& stock,
+                                                                               const sStkNPS& value)
+               {
+               int     i;
+               if(ifree.size()>0)
+                       { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
+                       else
+                       { i=stock.size();stock.push_back(value); }
+               return(i); 
+               }
+       //
+       private:
+                                       btDbvt(const btDbvt&)   {}      
+       };
+
+//
+// Inline's
+//
+
+//
+inline btDbvtAabbMm                    btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
+{
+btDbvtAabbMm box;
+box.mi=c-e;box.mx=c+e;
+return(box);
+}
+       
+//
+inline btDbvtAabbMm                    btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
+{
+return(FromCE(c,btVector3(r,r,r)));
+}
+       
+//
+inline btDbvtAabbMm                    btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
+{
+btDbvtAabbMm box;
+box.mi=mi;box.mx=mx;
+return(box);
+}
+       
+//
+inline btDbvtAabbMm                    btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
+{
+btDbvtAabbMm box;
+box.mi=box.mx=pts[0];
+for(int i=1;i<n;++i)
+       {
+       box.mi.setMin(pts[i]);
+       box.mx.setMax(pts[i]);
+       }
+return(box);
+}
+
+//
+inline btDbvtAabbMm                    btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
+{
+btDbvtAabbMm box;
+box.mi=box.mx=*ppts[0];
+for(int i=1;i<n;++i)
+       {
+       box.mi.setMin(*ppts[i]);
+       box.mx.setMax(*ppts[i]);
+       }
+return(box);
+}
+
+//
+DBVT_INLINE void               btDbvtAabbMm::Expand(const btVector3 e)
+{
+mi-=e;mx+=e;
+}
+       
+//
+DBVT_INLINE void               btDbvtAabbMm::SignedExpand(const btVector3 e)
+{
+if(e.x()>0) mx.setX(mx.x()+e.x()); else mi.setX(mi.x()+e.x());
+if(e.y()>0) mx.setY(mx.y()+e.y()); else mi.setY(mi.y()+e.y());
+if(e.z()>0) mx.setZ(mx.z()+e.z()); else mi.setZ(mi.z()+e.z());
+}
+       
+//
+DBVT_INLINE bool               btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
+{
+return(        (mi.x()<=a.mi.x())&&
+               (mi.y()<=a.mi.y())&&
+               (mi.z()<=a.mi.z())&&
+               (mx.x()>=a.mx.x())&&
+               (mx.y()>=a.mx.y())&&
+               (mx.z()>=a.mx.z()));
+}
+
+//
+DBVT_INLINE int                btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
+{
+btVector3                      pi,px;
+switch(s)
+       {
+       case    (0+0+0):        px=btVector3(mi.x(),mi.y(),mi.z());
+                                               pi=btVector3(mx.x(),mx.y(),mx.z());break;
+       case    (1+0+0):        px=btVector3(mx.x(),mi.y(),mi.z());
+                                               pi=btVector3(mi.x(),mx.y(),mx.z());break;
+       case    (0+2+0):        px=btVector3(mi.x(),mx.y(),mi.z());
+                                               pi=btVector3(mx.x(),mi.y(),mx.z());break;
+       case    (1+2+0):        px=btVector3(mx.x(),mx.y(),mi.z());
+                                               pi=btVector3(mi.x(),mi.y(),mx.z());break;
+       case    (0+0+4):        px=btVector3(mi.x(),mi.y(),mx.z());
+                                               pi=btVector3(mx.x(),mx.y(),mi.z());break;
+       case    (1+0+4):        px=btVector3(mx.x(),mi.y(),mx.z());
+                                               pi=btVector3(mi.x(),mx.y(),mi.z());break;
+       case    (0+2+4):        px=btVector3(mi.x(),mx.y(),mx.z());
+                                               pi=btVector3(mx.x(),mi.y(),mi.z());break;
+       case    (1+2+4):        px=btVector3(mx.x(),mx.y(),mx.z());
+                                               pi=btVector3(mi.x(),mi.y(),mi.z());break;
+       }
+if((dot(n,px)+o)<0)            return(-1);
+if((dot(n,pi)+o)>=0)   return(+1);
+return(0);
+}
+
+//
+DBVT_INLINE btScalar   btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
+{
+const btVector3*       b[]={&mx,&mi};
+const btVector3                p(      b[(signs>>0)&1]->x(),
+                                               b[(signs>>1)&1]->y(),
+                                               b[(signs>>2)&1]->z());
+return(dot(p,v));
+}
+
+//
+DBVT_INLINE void               btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
+{
+for(int i=0;i<3;++i)
+       {
+       if(d[i]<0)
+               { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
+               else
+               { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
+       }
+}
+       
+//
+DBVT_INLINE bool               Intersect(      const btDbvtAabbMm& a,
+                                                                       const btDbvtAabbMm& b)
+{
+return(        (a.mi.x()<=b.mx.x())&&
+               (a.mx.x()>=b.mi.x())&&
+               (a.mi.y()<=b.mx.y())&&
+               (a.mx.y()>=b.mi.y())&&
+               (a.mi.z()<=b.mx.z())&&          
+               (a.mx.z()>=b.mi.z()));
+}
+
+//
+DBVT_INLINE bool               Intersect(      const btDbvtAabbMm& a,
+                                                                       const btDbvtAabbMm& b,
+                                                                       const btTransform& xform)
+{
+const btVector3                d0=xform*b.Center()-a.Center();
+const btVector3                d1=d0*xform.getBasis();
+btScalar                       s0[2]={0,0};
+btScalar                       s1[2]={dot(xform.getOrigin(),d0),s1[0]};
+a.AddSpan(d0,s0[0],s0[1]);
+b.AddSpan(d1,s1[0],s1[1]);
+if(s0[0]>(s1[1])) return(false);
+if(s0[1]<(s1[0])) return(false);
+return(true);
+}
+
+//
+DBVT_INLINE bool               Intersect(      const btDbvtAabbMm& a,
+                                                                       const btVector3& b)
+{
+return(        (b.x()>=a.mi.x())&&
+               (b.y()>=a.mi.y())&&
+               (b.z()>=a.mi.z())&&
+               (b.x()<=a.mx.x())&&
+               (b.y()<=a.mx.y())&&
+               (b.z()<=a.mx.z()));
+}
+
+//
+DBVT_INLINE bool               Intersect(      const btDbvtAabbMm& a,
+                                                                       const btVector3& org,
+                                                                       const btVector3& invdir,
+                                                                       const unsigned* signs)
+{
+#if 0
+const btVector3                b0((a.mi-org)*invdir);
+const btVector3                b1((a.mx-org)*invdir);
+const btVector3                tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2]));
+const btVector3                tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2]));
+const btScalar         tin=btMax(tmin[0],btMax(tmin[1],tmin[2]));
+const btScalar         tout=btMin(tmax[0],btMin(tmax[1],tmax[2]));
+return(tin<tout);
+#else
+const btVector3*       bounds[2]={&a.mi,&a.mx};
+btScalar                       txmin=(bounds[  signs[0]]->x()-org[0])*invdir[0];
+btScalar                       txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0];
+const btScalar         tymin=(bounds[  signs[1]]->y()-org[1])*invdir[1];
+const btScalar         tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1];
+if((txmin>tymax)||(tymin>txmax)) return(false);
+if(tymin>txmin) txmin=tymin;
+if(tymax<txmax) txmax=tymax;
+const btScalar         tzmin=(bounds[  signs[2]]->z()-org[2])*invdir[2];
+const btScalar         tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2];
+if((txmin>tzmax)||(tzmin>txmax)) return(false);
+if(tzmin>txmin) txmin=tzmin;
+if(tzmax<txmax) txmax=tzmax;
+return(txmax>0);
+#endif
+}
+       
+//
+DBVT_INLINE btScalar   Proximity(      const btDbvtAabbMm& a,
+                                                                       const btDbvtAabbMm& b)
+{
+#if    DBVT_PROXIMITY_IMPL == DBVT_IMPL_SSE
+DBVT_ALIGN btScalar                                                    r[1];
+static DBVT_ALIGN const unsigned __int32       mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
+__asm
+       {
+       mov             eax,a
+       mov             ecx,b
+       movaps  xmm0,[eax]
+       movaps  xmm2,[ecx]
+       movaps  xmm1,[eax+16]
+       movaps  xmm3,[ecx+16]
+       addps   xmm0,xmm1
+       addps   xmm2,xmm3
+       subps   xmm0,xmm2
+       andps   xmm0,mask
+       movhlps xmm1,xmm0
+       addps   xmm0,xmm1
+       pshufd  xmm1,xmm0,1
+       addss   xmm0,xmm1
+       movss   r,xmm0
+       }
+return(r[0]);
+#else
+const btVector3        d=(a.mi+a.mx)-(b.mi+b.mx);
+return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
+#endif
+}
+
+//
+DBVT_INLINE int                        Select( const btDbvtAabbMm& o,
+                                                               const btDbvtAabbMm& a,
+                                                               const btDbvtAabbMm& b)
+{
+#if    DBVT_SELECT_IMPL == DBVT_IMPL_SSE
+DBVT_ALIGN __int32                                                     r[1];
+static DBVT_ALIGN const unsigned __int32       mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
+__asm
+       {
+       mov             eax,o
+       mov             ecx,a
+       mov             edx,b
+       movaps  xmm0,[eax]
+       movaps  xmm5,mask
+       addps   xmm0,[eax+16]   
+       movaps  xmm1,[ecx]
+       movaps  xmm2,[edx]
+       addps   xmm1,[ecx+16]
+       addps   xmm2,[edx+16]
+       subps   xmm1,xmm0
+       subps   xmm2,xmm0
+       andps   xmm1,xmm5
+       andps   xmm2,xmm5
+       movhlps xmm3,xmm1
+       movhlps xmm4,xmm2
+       addps   xmm1,xmm3
+       addps   xmm2,xmm4
+       pshufd  xmm3,xmm1,1
+       pshufd  xmm4,xmm2,1
+       addss   xmm1,xmm3
+       addss   xmm2,xmm4
+       cmpless xmm2,xmm1
+       movss   r,xmm2
+       }
+return(r[0]&1);
+#else
+return(Proximity(o,a)<Proximity(o,b)?0:1);
+#endif
+}
+
+//
+DBVT_INLINE void               Merge(  const btDbvtAabbMm& a,
+                                                               const btDbvtAabbMm& b,
+                                                               btDbvtAabbMm& r)
+{
+#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
+__asm
+       {
+       mov             eax,a
+       mov             edx,b
+       mov             ecx,r
+       movaps  xmm0,[eax+0]
+       movaps  xmm1,[edx+0]
+       movaps  xmm2,[eax+16]
+       movaps  xmm3,[edx+16]
+       minps   xmm0,xmm1
+       maxps   xmm2,xmm3
+       movaps  [ecx+0],xmm0
+       movaps  [ecx+16],xmm2
+       }
+#else
+for(int i=0;i<3;++i)
+       {
+       if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
+       if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
+       }
+#endif
+}
+
+//
+DBVT_INLINE bool               NotEqual(       const btDbvtAabbMm& a,
+                                                                       const btDbvtAabbMm& b)
+{
+return(        (a.mi.x()!=b.mi.x())||
+               (a.mi.y()!=b.mi.y())||
+               (a.mi.z()!=b.mi.z())||
+               (a.mx.x()!=b.mx.x())||
+               (a.mx.y()!=b.mx.y())||
+               (a.mx.z()!=b.mx.z()));
+}
+
+//
+// Inline's
+//
+
+//
+DBVT_PREFIX
+inline void            btDbvt::enumNodes(      const btDbvtNode* root,
+                                                                       DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+policy.Process(root);
+if(root->isinternal())
+       {
+       enumNodes(root->childs[0],policy);
+       enumNodes(root->childs[1],policy);
+       }
+}
+
+//
+DBVT_PREFIX
+inline void            btDbvt::enumLeaves(     const btDbvtNode* root,
+                                                                       DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root->isinternal())
+       {
+       enumLeaves(root->childs[0],policy);
+       enumLeaves(root->childs[1],policy);
+       }
+       else
+       {
+       policy.Process(root);
+       }
+}
+
+//
+DBVT_PREFIX
+inline void            btDbvt::collideTT(      const btDbvtNode* root0,
+                                                                       const btDbvtNode* root1,
+                                                                       DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root0&&root1)
+       {
+       btAlignedObjectArray<sStkNN>    stack;
+       int                                                             depth=1;
+       int                                                             treshold=DOUBLE_STACKSIZE-4;
+       stack.resize(DOUBLE_STACKSIZE);
+       stack[0]=sStkNN(root0,root1);
+       do      {
+               sStkNN  p=stack[--depth];
+               if(depth>treshold)
+                       {
+                       stack.resize(stack.size()*2);
+                       treshold=stack.size()-4;
+                       }
+               if(p.a==p.b)
+                       {
+                       if(p.a->isinternal())
+                               {
+                               stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
+                               stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
+                               stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+                               }
+                       }
+               else if(Intersect(p.a->volume,p.b->volume))
+                       {
+                       if(p.a->isinternal())
+                               {
+                               if(p.b->isinternal())
+                                       {
+                                       stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+                                       stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+                                       stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+                                       stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+                                       }
+                                       else
+                                       {
+                                       stack[depth++]=sStkNN(p.a->childs[0],p.b);
+                                       stack[depth++]=sStkNN(p.a->childs[1],p.b);
+                                       }
+                               }
+                               else
+                               {
+                               if(p.b->isinternal())
+                                       {
+                                       stack[depth++]=sStkNN(p.a,p.b->childs[0]);
+                                       stack[depth++]=sStkNN(p.a,p.b->childs[1]);
+                                       }
+                                       else
+                                       {
+                                       policy.Process(p.a,p.b);
+                                       }
+                               }
+                       }
+               } while(depth);
+       }
+}
+
+//
+DBVT_PREFIX
+inline void            btDbvt::collideTT(      const btDbvtNode* root0,
+                                                                       const btDbvtNode* root1,
+                                                                       const btTransform& xform,
+                                                                       DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root0&&root1)
+       {
+       btAlignedObjectArray<sStkNN>    stack;
+       int                                                             depth=1;
+       int                                                             treshold=DOUBLE_STACKSIZE-4;
+       stack.resize(DOUBLE_STACKSIZE);
+       stack[0]=sStkNN(root0,root1);
+       do      {
+               sStkNN  p=stack[--depth];
+               if(Intersect(p.a->volume,p.b->volume,xform))
+                       {
+                       if(depth>treshold)
+                               {
+                               stack.resize(stack.size()*2);
+                               treshold=stack.size()-4;
+                               }
+                       if(p.a->isinternal())
+                               {
+                               if(p.b->isinternal())
+                                       {                                       
+                                       stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+                                       stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+                                       stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+                                       stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+                                       }
+                                       else
+                                       {
+                                       stack[depth++]=sStkNN(p.a->childs[0],p.b);
+                                       stack[depth++]=sStkNN(p.a->childs[1],p.b);
+                                       }
+                               }
+                               else
+                               {
+                               if(p.b->isinternal())
+                                       {
+                                       stack[depth++]=sStkNN(p.a,p.b->childs[0]);
+                                       stack[depth++]=sStkNN(p.a,p.b->childs[1]);
+                                       }
+                                       else
+                                       {
+                                       policy.Process(p.a,p.b);
+                                       }
+                               }
+                       }
+               } while(depth);
+       }
+}
+
+//
+DBVT_PREFIX
+inline void            btDbvt::collideTT(      const btDbvtNode* root0,
+                                                                       const btTransform& xform0,
+                                                                       const btDbvtNode* root1,
+                                                                       const btTransform& xform1,
+                                                                       DBVT_IPOLICY)
+{
+const btTransform      xform=xform0.inverse()*xform1;
+collideTT(root0,root1,xform,policy);
+}
+
+//
+DBVT_PREFIX
+inline void            btDbvt::collideTV(      const btDbvtNode* root,
+                                                                       const btDbvtVolume& volume,
+                                                                       DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root)
+       {
+       btAlignedObjectArray<const btDbvtNode*> stack;
+       stack.reserve(SIMPLE_STACKSIZE);
+       stack.push_back(root);
+       do      {
+               const btDbvtNode*       n=stack[stack.size()-1];
+               stack.pop_back();
+               if(Intersect(n->volume,volume))
+                       {
+                       if(n->isinternal())
+                               {
+                               stack.push_back(n->childs[0]);
+                               stack.push_back(n->childs[1]);
+                               }
+                               else
+                               {
+                               policy.Process(n);
+                               }
+                       }
+               } while(stack.size()>0);
+       }
+}
+
+//
+DBVT_PREFIX
+inline void            btDbvt::collideRAY(     const btDbvtNode* root,
+                                                                       const btVector3& origin,
+                                                                       const btVector3& direction,
+                                                                       DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root)
+       {
+       const btVector3 normal=direction.normalized();
+       const btVector3 invdir( 1/normal.x(),
+                                                       1/normal.y(),
+                                                       1/normal.z());
+       const unsigned  signs[]={       direction.x()<0,
+                                                               direction.y()<0,
+                                                               direction.z()<0};
+       btAlignedObjectArray<const btDbvtNode*> stack;
+       stack.reserve(SIMPLE_STACKSIZE);
+       stack.push_back(root);
+       do      {
+               const btDbvtNode*       node=stack[stack.size()-1];
+               stack.pop_back();
+               if(Intersect(node->volume,origin,invdir,signs))
+                       {
+                       if(node->isinternal())
+                               {
+                               stack.push_back(node->childs[0]);
+                               stack.push_back(node->childs[1]);
+                               }
+                               else
+                               {
+                               policy.Process(node);
+                               }
+                       }
+               } while(stack.size());
+       }
+}
+
+//
+DBVT_PREFIX
+inline void            btDbvt::collideKDOP(const btDbvtNode* root,
+                                                                       const btVector3* normals,
+                                                                       const btScalar* offsets,
+                                                                       int count,
+                                                                       DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root)
+       {
+       const int                                               inside=(1<<count)-1;
+       btAlignedObjectArray<sStkNP>    stack;
+       int                                                             signs[sizeof(unsigned)*8];
+       btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
+       for(int i=0;i<count;++i)
+               {
+               signs[i]=       ((normals[i].x()>=0)?1:0)+
+                                       ((normals[i].y()>=0)?2:0)+
+                                       ((normals[i].z()>=0)?4:0);
+               }
+       stack.reserve(SIMPLE_STACKSIZE);
+       stack.push_back(sStkNP(root,0));
+       do      {
+               sStkNP  se=stack[stack.size()-1];
+               bool    out=false;
+               stack.pop_back();
+               for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+                       {
+                       if(0==(se.mask&j))
+                               {
+                               const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
+                               switch(side)
+                                       {
+                                       case    -1:     out=true;break;
+                                       case    +1:     se.mask|=j;break;
+                                       }
+                               }
+                       }
+               if(!out)
+                       {
+                       if((se.mask!=inside)&&(se.node->isinternal()))
+                               {
+                               stack.push_back(sStkNP(se.node->childs[0],se.mask));
+                               stack.push_back(sStkNP(se.node->childs[1],se.mask));
+                               }
+                               else
+                               {
+                               if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
+                               }
+                       }
+               } while(stack.size());
+       }
+}
+
+//
+DBVT_PREFIX
+inline void            btDbvt::collideOCL(     const btDbvtNode* root,
+                                                                       const btVector3* normals,
+                                                                       const btScalar* offsets,
+                                                                       const btVector3& sortaxis,
+                                                                       int count,
+                                                                       DBVT_IPOLICY,
+                                                                       bool fsort)
+{
+DBVT_CHECKTYPE
+if(root)
+       {
+       const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
+                                                                                       (sortaxis[1]>=0?2:0)+
+                                                                                       (sortaxis[2]>=0?4:0);
+       const int                                               inside=(1<<count)-1;
+       btAlignedObjectArray<sStkNPS>   stock;
+       btAlignedObjectArray<int>               ifree;
+       btAlignedObjectArray<int>               stack;
+       int                                                             signs[sizeof(unsigned)*8];
+       btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
+       for(int i=0;i<count;++i)
+               {
+               signs[i]=       ((normals[i].x()>=0)?1:0)+
+                                       ((normals[i].y()>=0)?2:0)+
+                                       ((normals[i].z()>=0)?4:0);
+               }
+       stock.reserve(SIMPLE_STACKSIZE);
+       stack.reserve(SIMPLE_STACKSIZE);
+       ifree.reserve(SIMPLE_STACKSIZE);
+       stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
+       do      {
+               const int       id=stack[stack.size()-1];
+               sStkNPS         se=stock[id];
+               stack.pop_back();ifree.push_back(id);
+               if(se.mask!=inside)
+                       {
+                       bool    out=false;
+                       for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+                               {
+                               if(0==(se.mask&j))
+                                       {
+                                       const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
+                                       switch(side)
+                                               {
+                                               case    -1:     out=true;break;
+                                               case    +1:     se.mask|=j;break;
+                                               }
+                                       }
+                               }
+                       if(out) continue;
+                       }
+               if(policy.Descent(se.node))
+                       {
+                       if(se.node->isinternal())
+                               {
+                               const btDbvtNode* pns[]={       se.node->childs[0],se.node->childs[1]};
+                               sStkNPS         nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
+                                                                       sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
+                               const int       q=nes[0].value<nes[1].value?1:0;                                
+                               int                     j=stack.size();
+                               if(fsort&&(j>0))
+                                       {
+                                       /* Insert 0     */ 
+                                       j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
+                                       stack.push_back(0);
+                                       #if DBVT_USE_MEMMOVE
+                                       memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
+                                       #else
+                                       for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
+                                       #endif
+                                       stack[j]=allocate(ifree,stock,nes[q]);
+                                       /* Insert 1     */ 
+                                       j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
+                                       stack.push_back(0);
+                                       #if DBVT_USE_MEMMOVE
+                                       memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
+                                       #else
+                                       for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
+                                       #endif
+                                       stack[j]=allocate(ifree,stock,nes[1-q]);
+                                       }
+                                       else
+                                       {
+                                       stack.push_back(allocate(ifree,stock,nes[q]));
+                                       stack.push_back(allocate(ifree,stock,nes[1-q]));
+                                       }
+                               }
+                               else
+                               {
+                               policy.Process(se.node,se.value);
+                               }
+                       }
+               } while(stack.size());
+       }
+}
+
+//
+DBVT_PREFIX
+inline void            btDbvt::collideTU(      const btDbvtNode* root,
+                                                                       DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root)
+       {
+       btAlignedObjectArray<const btDbvtNode*> stack;
+       stack.reserve(SIMPLE_STACKSIZE);
+       stack.push_back(root);
+       do      {
+               const btDbvtNode*       n=stack[stack.size()-1];
+               stack.pop_back();
+               if(policy.Descent(n))
+                       {
+                       if(n->isinternal())
+                               { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
+                               else
+                               { policy.Process(n); }
+                       }
+               } while(stack.size()>0);
+       }
+}
+
+//
+// PP Cleanup
+//
+
+#undef DBVT_USE_MEMMOVE
+#undef DBVT_USE_TEMPLATE
+#undef DBVT_VIRTUAL_DTOR
+#undef DBVT_VIRTUAL
+#undef DBVT_PREFIX
+#undef DBVT_IPOLICY
+#undef DBVT_CHECKTYPE
+#undef DBVT_IMPL_GENERIC
+#undef DBVT_IMPL_FPU0x86
+#undef DBVT_IMPL_SSE
+
+#endif
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
new file mode 100644 (file)
index 0000000..c6086f2
--- /dev/null
@@ -0,0 +1,344 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 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.
+*/
+///btDbvtBroadphase implementation by Nathanael Presson
+
+#include "btDbvtBroadphase.h"
+
+//
+// Profiling
+//
+
+#if DBVT_BP_PROFILE
+#include <stdio.h>
+struct ProfileScope
+       {
+       ProfileScope(btClock& clock,unsigned long& value)
+               {
+               m_clock=&clock;
+               m_value=&value;
+               m_base=clock.getTimeMicroseconds();
+               }
+       ~ProfileScope()
+               {
+               (*m_value)+=m_clock->getTimeMicroseconds()-m_base;
+               }
+       btClock*                m_clock;
+       unsigned long*  m_value;
+       unsigned long   m_base;
+       };
+#define        SPC(_value_)    ProfileScope    spc_scope(m_clock,_value_)
+#else
+#define        SPC(_value_)
+#endif
+
+//
+// Helpers
+//
+
+//
+template <typename T>
+static inline void     listappend(T* item,T*& list)
+{
+item->links[0]=0;
+item->links[1]=list;
+if(list) list->links[0]=item;
+list=item;
+}
+
+//
+template <typename T>
+static inline void     listremove(T* item,T*& list)
+{
+if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
+if(item->links[1]) item->links[1]->links[0]=item->links[0];
+}
+
+//
+template <typename T>
+static inline int      listcount(T* root)
+{
+int    n=0;
+while(root) { ++n;root=root->links[1]; }
+return(n);
+}
+
+//
+template <typename T>
+static inline void     clear(T& value)
+{
+static const struct ZeroDummy : T {} zerodummy;
+value=zerodummy;
+}
+
+//
+// Colliders
+//
+
+/* Tree collider       */ 
+struct btDbvtTreeCollider : btDbvt::ICollide
+{
+btDbvtBroadphase*      pbp;
+               btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
+void   Process(const btDbvtNode* na,const btDbvtNode* nb)
+       {
+       btDbvtProxy*    pa=(btDbvtProxy*)na->data;
+       btDbvtProxy*    pb=(btDbvtProxy*)nb->data;
+       #if DBVT_BP_DISCRETPAIRS
+       if(Intersect(pa->aabb,pb->aabb))
+       #endif
+               {
+               if(pa>pb) btSwap(pa,pb);
+               pbp->m_paircache->addOverlappingPair(pa,pb);
+               }
+       }
+};
+
+//
+// btDbvtBroadphase
+//
+
+//
+btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
+{
+m_releasepaircache     =       (paircache!=0)?false:true;
+m_predictedframes      =       2;
+m_stageCurrent         =       0;
+m_fupdates                     =       1;
+m_dupdates                     =       1;
+m_paircache                    =       paircache?
+                                                       paircache       :
+                                                       new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+m_gid                          =       0;
+m_pid                          =       0;
+for(int i=0;i<=STAGECOUNT;++i)
+       {
+       m_stageRoots[i]=0;
+       }
+#if DBVT_BP_PROFILE
+clear(m_profiling);
+#endif
+}
+
+//
+btDbvtBroadphase::~btDbvtBroadphase()
+{
+if(m_releasepaircache) 
+{
+       m_paircache->~btOverlappingPairCache();
+       btAlignedFree(m_paircache);
+}
+}
+
+//
+btBroadphaseProxy*                             btDbvtBroadphase::createProxy(  const btVector3& aabbMin,
+                                                                                                                               const btVector3& aabbMax,
+                                                                                                                               int /*shapeType*/,
+                                                                                                                               void* userPtr,
+                                                                                                                               short int collisionFilterGroup,
+                                                                                                                               short int collisionFilterMask,
+                                                                                                                               btDispatcher* /*dispatcher*/,
+                                                                                                                               void* /*multiSapProxy*/)
+{
+btDbvtProxy*   proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(  userPtr,
+                                                                                                                                                               collisionFilterGroup,
+                                                                                                                                                               collisionFilterMask);
+proxy->aabb                    =       btDbvtVolume::FromMM(aabbMin,aabbMax);
+proxy->leaf                    =       m_sets[0].insert(proxy->aabb,proxy);
+proxy->stage           =       m_stageCurrent;
+proxy->m_uniqueId      =       ++m_gid;
+listappend(proxy,m_stageRoots[m_stageCurrent]);
+return(proxy);
+}
+
+//
+void                                                   btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
+                                                                                                                               btDispatcher* dispatcher)
+{
+btDbvtProxy*   proxy=(btDbvtProxy*)absproxy;
+if(proxy->stage==STAGECOUNT)
+       m_sets[1].remove(proxy->leaf);
+       else
+       m_sets[0].remove(proxy->leaf);
+listremove(proxy,m_stageRoots[proxy->stage]);
+m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
+btAlignedFree(proxy);
+}
+
+//
+void                                                   btDbvtBroadphase::setAabb(              btBroadphaseProxy* absproxy,
+                                                                                                                               const btVector3& aabbMin,
+                                                                                                                               const btVector3& aabbMax,
+                                                                                                                               btDispatcher* /*dispatcher*/)
+{
+btDbvtProxy*   proxy=(btDbvtProxy*)absproxy;
+btDbvtVolume   aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
+if(NotEqual(aabb,proxy->leaf->volume))
+       {
+       if(proxy->stage==STAGECOUNT)
+               {/* fixed -> dynamic set        */ 
+               m_sets[1].remove(proxy->leaf);
+               proxy->leaf=m_sets[0].insert(aabb,proxy);
+               }
+               else
+               {/* dynamic set                         */ 
+               if(Intersect(proxy->leaf->volume,aabb))
+                       {/* Moving                              */ 
+                       const btVector3 delta=(aabbMin+aabbMax)/2-proxy->aabb.Center();
+                       #ifdef DBVT_BP_MARGIN
+                       m_sets[0].update(proxy->leaf,aabb,delta*m_predictedframes,DBVT_BP_MARGIN);
+                       #else
+                       m_sets[0].update(proxy->leaf,aabb,delta*m_predictedframes);
+                       #endif
+                       }
+                       else
+                       {/* Teleporting                 */ 
+                       m_sets[0].update(proxy->leaf,aabb);             
+                       }       
+               }
+       listremove(proxy,m_stageRoots[proxy->stage]);
+       proxy->aabb             =       aabb;
+       proxy->stage    =       m_stageCurrent;
+       listappend(proxy,m_stageRoots[m_stageCurrent]);
+       }
+}
+
+//
+void                                                   btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+collide(dispatcher);
+#if DBVT_BP_PROFILE
+if(0==(m_pid%DBVT_BP_PROFILING_RATE))
+       {       
+       printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
+       unsigned int    total=m_profiling.m_total;
+       if(total<=0) total=1;
+       printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
+       printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
+       printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
+       printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
+       const unsigned long     sum=m_profiling.m_ddcollide+
+                                                       m_profiling.m_fdcollide+
+                                                       m_profiling.m_cleanup;
+       printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
+       printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE));
+       clear(m_profiling);
+       m_clock.reset();
+       }
+#endif
+}
+
+//
+void                                                   btDbvtBroadphase::collide(btDispatcher* dispatcher)
+{
+SPC(m_profiling.m_total);
+/* optimize                            */ 
+m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
+m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
+/* dynamic -> fixed set        */ 
+m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
+btDbvtProxy*   current=m_stageRoots[m_stageCurrent];
+if(current)
+       {
+       btDbvtTreeCollider      collider(this);
+       do      {
+               btDbvtProxy*    next=current->links[1];
+               listremove(current,m_stageRoots[current->stage]);
+               listappend(current,m_stageRoots[STAGECOUNT]);
+               btDbvt::collideTT(m_sets[1].m_root,current->leaf,collider);
+               m_sets[0].remove(current->leaf);
+               current->leaf   =       m_sets[1].insert(current->aabb,current);
+               current->stage  =       STAGECOUNT;     
+               current                 =       next;
+               } while(current);
+       }
+/* collide dynamics            */ 
+       {
+       btDbvtTreeCollider      collider(this);
+               {
+               SPC(m_profiling.m_fdcollide);
+               btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
+               }
+               {
+               SPC(m_profiling.m_ddcollide);
+               btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
+               }
+       }
+/* clean up                            */ 
+       {
+       SPC(m_profiling.m_cleanup);
+       btBroadphasePairArray&  pairs=m_paircache->getOverlappingPairArray();
+       if(pairs.size()>0)
+               {
+               for(int i=0,ni=pairs.size();i<ni;++i)
+                       {
+                       btBroadphasePair&       p=pairs[i];
+                       btDbvtProxy*    pa=(btDbvtProxy*)p.m_pProxy0;
+                       btDbvtProxy*    pb=(btDbvtProxy*)p.m_pProxy1;
+                       if(!Intersect(pa->aabb,pb->aabb))
+                               {
+                               if(pa>pb) btSwap(pa,pb);
+                               m_paircache->removeOverlappingPair(pa,pb,dispatcher);
+                               --ni;--i;
+                               }
+                       }
+               }
+       }
+++m_pid;
+}
+
+//
+void                                                   btDbvtBroadphase::optimize()
+{
+m_sets[0].optimizeTopDown();
+m_sets[1].optimizeTopDown();
+}
+
+//
+btOverlappingPairCache*                        btDbvtBroadphase::getOverlappingPairCache()
+{
+return(m_paircache);
+}
+
+//
+const btOverlappingPairCache*  btDbvtBroadphase::getOverlappingPairCache() const
+{
+return(m_paircache);
+}
+
+//
+void                                                   btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+{
+
+       ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds;
+
+if(!m_sets[0].empty())
+       if(!m_sets[1].empty())  Merge(  m_sets[0].m_root->volume,
+                                                                       m_sets[1].m_root->volume,bounds);
+                                                       else
+                                                       bounds=m_sets[0].m_root->volume;
+else if(!m_sets[1].empty())    bounds=m_sets[1].m_root->volume;
+                                                       else
+                                                       bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
+aabbMin=bounds.Mins();
+aabbMax=bounds.Maxs();
+}
+
+//
+void                                                   btDbvtBroadphase::printStats()
+{}
+
+#if DBVT_BP_PROFILE
+#undef SPC
+#endif
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
new file mode 100644 (file)
index 0000000..3f19075
--- /dev/null
@@ -0,0 +1,103 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 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.
+*/
+///btDbvtBroadphase implementation by Nathanael Presson
+#ifndef BT_DBVT_BROADPHASE_H
+#define BT_DBVT_BROADPHASE_H
+
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+
+//
+// Compile time config
+//
+
+#define        DBVT_BP_PROFILE                 0
+#define DBVT_BP_DISCRETPAIRS   1
+#define DBVT_BP_MARGIN                 (btScalar)0.05
+
+#if DBVT_BP_PROFILE
+       #define DBVT_BP_PROFILING_RATE  256
+       #include "LinearMath/btQuickprof.h"
+#endif
+
+//
+// btDbvtProxy
+//
+struct btDbvtProxy : btBroadphaseProxy
+{
+/* Fields              */ 
+btDbvtAabbMm           aabb;
+btDbvtNode*            leaf;
+btDbvtProxy*           links[2];
+int                                    stage;
+/* ctor                        */ 
+btDbvtProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
+       btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask)
+       {
+       links[0]=links[1]=0;
+       }
+};
+
+typedef btAlignedObjectArray<btDbvtProxy*>     btDbvtProxyArray;
+
+///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt).
+///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
+///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3.
+struct btDbvtBroadphase : btBroadphaseInterface
+{
+/* Config              */ 
+enum   {
+               DYNAMIC_SET                     =       0,      /* Dynamic set index    */ 
+               FIXED_SET                       =       1,      /* Fixed set index              */ 
+               STAGECOUNT                      =       2       /* Number of stages             */ 
+               };
+/* Fields              */ 
+btDbvt                                 m_sets[2];                                      // Dbvt sets
+btDbvtProxy*                   m_stageRoots[STAGECOUNT+1];     // Stages list
+btOverlappingPairCache*        m_paircache;                            // Pair cache
+btScalar                               m_predictedframes;                      // Frames predicted
+int                                            m_stageCurrent;                         // Current stage
+int                                            m_fupdates;                                     // % of fixed updates per frame
+int                                            m_dupdates;                                     // % of dynamic updates per frame
+int                                            m_pid;                                          // Parse id
+int                                            m_gid;                                          // Gen id
+bool                                   m_releasepaircache;                     // Release pair cache on delete
+#if DBVT_BP_PROFILE
+btClock                                        m_clock;
+struct {
+               unsigned long           m_total;
+               unsigned long           m_ddcollide;
+               unsigned long           m_fdcollide;
+               unsigned long           m_cleanup;
+               unsigned long           m_jobcount;
+               }                               m_profiling;
+#endif
+/* Methods             */ 
+btDbvtBroadphase(btOverlappingPairCache* paircache=0);
+~btDbvtBroadphase();
+void                                                   collide(btDispatcher* dispatcher);
+void                                                   optimize();
+/* btBroadphaseInterface Implementation        */ 
+btBroadphaseProxy*                             createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+void                                                   destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+void                                                   setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+void                                                   calculateOverlappingPairs(btDispatcher* dispatcher);
+btOverlappingPairCache*                        getOverlappingPairCache();
+const btOverlappingPairCache*  getOverlappingPairCache() const;
+void                                                   getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
+void                                                   printStats();
+};
+
+#endif
index 3d958cc8fef19f75310c3cd9a8c3b3ce51f0ab2c..6db71a0170eb89a54bb8c4fb86d3a37f8e3459a7 100644 (file)
@@ -16,7 +16,7 @@ subject to the following restrictions:
 #ifndef _DISPATCHER_H
 #define _DISPATCHER_H
 
-#include "../../LinearMath/btScalar.h"
+#include "LinearMath/btScalar.h"
 
 class btCollisionAlgorithm;
 struct btBroadphaseProxy;
@@ -43,7 +43,9 @@ struct btDispatcherInfo
                m_useContinuous(false),
                m_debugDraw(0),
                m_enableSatConvex(false),
-               m_enableSPU(false),
+               m_enableSPU(true),
+               m_useEpa(true),
+               m_allowedCcdPenetration(btScalar(0.04)),
                m_stackAllocator(0)
        {
 
@@ -51,17 +53,19 @@ struct btDispatcherInfo
        btScalar        m_timeStep;
        int             m_stepCount;
        int             m_dispatchFunc;
-       btScalar        m_timeOfImpact;
+       mutable btScalar        m_timeOfImpact;
        bool    m_useContinuous;
        class btIDebugDraw*     m_debugDraw;
        bool    m_enableSatConvex;
        bool    m_enableSPU;
+       bool    m_useEpa;
+       btScalar        m_allowedCcdPenetration;
        btStackAlloc*   m_stackAllocator;
        
 };
 
-/// btDispatcher can be used in combination with broadphase to dispatch overlapping pairs.
-/// For example for pairwise collision detection or user callbacks (game logic).
+///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.
+///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic).
 class btDispatcher
 {
 
@@ -81,12 +85,18 @@ public:
 
        virtual bool    needsResponse(btCollisionObject* body0,btCollisionObject* body1)=0;
 
-       virtual void    dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)=0;
+       virtual void    dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher)  =0;
 
        virtual int getNumManifolds() const = 0;
 
        virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0;
 
+       virtual btPersistentManifold**  getInternalManifoldPointer() = 0;
+
+       virtual void* allocateCollisionAlgorithm(int size)  = 0;
+
+       virtual void freeCollisionAlgorithm(void* ptr) = 0;
+
 };
 
 
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
new file mode 100644 (file)
index 0000000..3f866ab
--- /dev/null
@@ -0,0 +1,466 @@
+/*
+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 "btMultiSapBroadphase.h"
+
+#include "btSimpleBroadphase.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "btQuantizedBvh.h"
+
+///    btSapBroadphaseArray    m_sapBroadphases;
+
+///    btOverlappingPairCache* m_overlappingPairs;
+extern int gOverlappingPairs;
+
+/*
+class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
+{
+public:
+
+       virtual btBroadphasePair*       addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+       {
+               return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy);
+       }
+};
+
+*/
+
+btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache)
+:m_overlappingPairs(pairCache),
+m_optimizedAabbTree(0),
+m_ownsPairCache(false),
+m_invalidPair(0)
+{
+       if (!m_overlappingPairs)
+       {
+               m_ownsPairCache = true;
+               void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16);
+               m_overlappingPairs = new (mem)btSortedOverlappingPairCache();
+       }
+
+       struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
+       {
+               virtual ~btMultiSapOverlapFilterCallback()
+               {}
+               // return true when pairs need collision
+               virtual bool    needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
+               {
+                       btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
+                       btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
+                       
+                       bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
+                       collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
+       
+                       return collides;
+               }
+       };
+
+       void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16);
+       m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
+
+       m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
+//     mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
+//     m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
+}
+
+btMultiSapBroadphase::~btMultiSapBroadphase()
+{
+       if (m_ownsPairCache)
+       {
+               m_overlappingPairs->~btOverlappingPairCache();
+               btAlignedFree(m_overlappingPairs);
+       }
+}
+
+
+void   btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax)
+{
+       m_optimizedAabbTree = new btQuantizedBvh();
+       m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax);
+       QuantizedNodeArray&     nodes = m_optimizedAabbTree->getLeafNodeArray();
+       for (int i=0;i<m_sapBroadphases.size();i++)
+       {
+               btQuantizedBvhNode node;
+               btVector3 aabbMin,aabbMax;
+               m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax);
+               m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
+               m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
+               int partId = 0;
+               node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i;
+               nodes.push_back(node);
+       }
+       m_optimizedAabbTree->buildInternal();
+}
+
+btBroadphaseProxy*     btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/)
+{
+       //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
+
+       void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
+       btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
+       m_multiSapProxies.push_back(proxy);
+
+       ///this should deal with inserting/removal into child broadphases
+       setAabb(proxy,aabbMin,aabbMax,dispatcher);
+       return proxy;
+}
+
+void   btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
+{
+       ///not yet
+       btAssert(0);
+
+}
+
+
+void   btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*  childBroadphase)
+{
+       void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16);
+       btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy;
+       bridgeProxyRef->m_childProxy = childProxy;
+       bridgeProxyRef->m_childBroadphase = childBroadphase;
+       parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef);
+}
+
+
+bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax);
+bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
+{
+return
+amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
+amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
+amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
+}
+
+
+
+
+
+
+//#include <stdio.h>
+
+void   btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
+{
+       btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
+       multiProxy->m_aabbMin = aabbMin;
+       multiProxy->m_aabbMax = aabbMax;
+       
+       
+//     bool fullyContained = false;
+//     bool alreadyInSimple = false;
+       
+
+
+       
+       struct MyNodeOverlapCallback : public btNodeOverlapCallback
+       {
+               btMultiSapBroadphase*   m_multiSap;
+               btMultiSapProxy*                m_multiProxy;
+               btDispatcher*                   m_dispatcher;
+
+               MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
+                       :m_multiSap(multiSap),
+                       m_multiProxy(multiProxy),
+                       m_dispatcher(dispatcher)
+               {
+
+               }
+
+               virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex)
+               {
+                       btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
+
+                       int containingBroadphaseIndex = -1;
+                       //already found?
+                       for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
+                       {
+
+                               if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
+                               {
+                                       containingBroadphaseIndex = i;
+                                       break;
+                               }
+                       }
+                       if (containingBroadphaseIndex<0)
+                       {
+                               //add it
+                               btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy);
+                               m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
+
+                       }
+               }
+       };
+
+       MyNodeOverlapCallback   myNodeCallback(this,multiProxy,dispatcher);
+
+
+
+       
+       m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
+       int i;
+
+       for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
+       {
+               btVector3 worldAabbMin,worldAabbMax;
+               multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
+               bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
+               if (!overlapsBroadphase)
+               {
+                       //remove it now
+                       btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
+
+                       btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
+                       bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
+                       
+                       multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
+                       multiProxy->m_bridgeProxies.pop_back();
+
+               }
+       }
+
+
+       /*
+
+       if (1)
+       {
+
+               //find broadphase that contain this multiProxy
+               int numChildBroadphases = getBroadphaseArray().size();
+               for (int i=0;i<numChildBroadphases;i++)
+               {
+                       btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
+                       btVector3 worldAabbMin,worldAabbMax;
+                       childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
+                       bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
+                       
+               //      fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
+                       int containingBroadphaseIndex = -1;
+                       
+                       //if already contains this
+                       
+                       for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
+                       {
+                               if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
+                               {
+                                       containingBroadphaseIndex = i;
+                               }
+                               alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
+                       }
+
+                       if (overlapsBroadphase)
+                       {
+                               if (containingBroadphaseIndex<0)
+                               {
+                                       btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
+                                       childProxy->m_multiSapParentProxy = multiProxy;
+                                       addToChildBroadphase(multiProxy,childProxy,childBroadphase);
+                               }
+                       } else
+                       {
+                               if (containingBroadphaseIndex>=0)
+                               {
+                                       //remove
+                                       btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
+
+                                       btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
+                                       bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
+                                       
+                                       multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
+                                       multiProxy->m_bridgeProxies.pop_back();
+                               }
+                       }
+               }
+
+
+               ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force)
+               ///hopefully we don't end up with many entries here (can assert/provide feedback on stats)
+               if (0)//!multiProxy->m_bridgeProxies.size())
+               {
+                       ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
+                       ///this is needed to be able to calculate the aabb overlap
+                       btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
+                       childProxy->m_multiSapParentProxy = multiProxy;
+                       addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
+               }
+       }
+
+       if (!multiProxy->m_bridgeProxies.size())
+       {
+               ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
+               ///this is needed to be able to calculate the aabb overlap
+               btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
+               childProxy->m_multiSapParentProxy = multiProxy;
+               addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
+       }
+*/
+
+
+       //update
+       for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
+       {
+               btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
+               bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
+       }
+
+}
+bool stopUpdating=false;
+
+
+
+class btMultiSapBroadphasePairSortPredicate
+{
+       public:
+
+               bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 )
+               {
+                               btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0;
+                               btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0;
+                               btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0;
+                               btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
+
+                                return aProxy0 > bProxy0 || 
+                                       (aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
+                                       (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); 
+               }
+};
+
+
+        ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+void    btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+
+//     m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
+
+       if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
+       {
+       
+               btBroadphasePairArray&  overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
+
+       //      quicksort(overlappingPairArray,0,overlappingPairArray.size());
+
+               overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
+
+               //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+       //      overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
+
+               overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+               m_invalidPair = 0;
+
+               
+               int i;
+
+               btBroadphasePair previousPair;
+               previousPair.m_pProxy0 = 0;
+               previousPair.m_pProxy1 = 0;
+               previousPair.m_algorithm = 0;
+               
+               
+               for (i=0;i<overlappingPairArray.size();i++)
+               {
+               
+                       btBroadphasePair& pair = overlappingPairArray[i];
+
+                       btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
+                       btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0;
+                       btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
+                       btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
+
+                       bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
+                       
+                       previousPair = pair;
+
+                       bool needsRemoval = false;
+
+                       if (!isDuplicate)
+                       {
+                               bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+                               if (hasOverlap)
+                               {
+                                       needsRemoval = false;//callback->processOverlap(pair);
+                               } else
+                               {
+                                       needsRemoval = true;
+                               }
+                       } else
+                       {
+                               //remove duplicate
+                               needsRemoval = true;
+                               //should have no algorithm
+                               btAssert(!pair.m_algorithm);
+                       }
+                       
+                       if (needsRemoval)
+                       {
+                               getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
+
+               //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+               //              m_overlappingPairArray.pop_back();
+                               pair.m_pProxy0 = 0;
+                               pair.m_pProxy1 = 0;
+                               m_invalidPair++;
+                               gOverlappingPairs--;
+                       } 
+                       
+               }
+
+       ///if you don't like to skip the invalid pairs in the array, execute following code:
+       #define CLEAN_INVALID_PAIRS 1
+       #ifdef CLEAN_INVALID_PAIRS
+
+               //perform a sort, to sort 'invalid' pairs to the end
+               //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
+               overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
+
+               overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+               m_invalidPair = 0;
+       #endif//CLEAN_INVALID_PAIRS
+               
+               //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
+       }
+
+
+}
+
+
+bool   btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
+{
+       btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
+               btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
+
+               return  TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
+                       multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
+               
+}
+
+
+void   btMultiSapBroadphase::printStats()
+{
+/*     printf("---------------------------------\n");
+       
+               printf("btMultiSapBroadphase.h\n");
+               printf("numHandles = %d\n",m_multiSapProxies.size());
+                       //find broadphase that contain this multiProxy
+               int numChildBroadphases = getBroadphaseArray().size();
+               for (int i=0;i<numChildBroadphases;i++)
+               {
+
+                       btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
+                       childBroadphase->printStats();
+
+               }
+               */
+
+}
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
new file mode 100644 (file)
index 0000000..a0c002d
--- /dev/null
@@ -0,0 +1,144 @@
+/*
+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_MULTI_SAP_BROADPHASE
+#define BT_MULTI_SAP_BROADPHASE
+
+#include "btBroadphaseInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btOverlappingPairCache.h"
+
+
+class btBroadphaseInterface;
+class btSimpleBroadphase;
+
+
+typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray;
+
+///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases.
+///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time.
+///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy.
+///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328
+///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329
+class btMultiSapBroadphase :public btBroadphaseInterface
+{
+       btSapBroadphaseArray    m_sapBroadphases;
+       
+       btSimpleBroadphase*             m_simpleBroadphase;
+
+       btOverlappingPairCache* m_overlappingPairs;
+
+       class btQuantizedBvh*                   m_optimizedAabbTree;
+
+
+       bool                                    m_ownsPairCache;
+       
+       btOverlapFilterCallback*        m_filterCallback;
+
+       int                     m_invalidPair;
+
+       struct  btBridgeProxy
+       {
+               btBroadphaseProxy*              m_childProxy;
+               btBroadphaseInterface*  m_childBroadphase;
+       };
+
+
+public:
+
+       struct  btMultiSapProxy : public btBroadphaseProxy
+       {
+
+               ///array with all the entries that this proxy belongs to
+               btAlignedObjectArray<btBridgeProxy*> m_bridgeProxies;
+               btVector3       m_aabbMin;
+               btVector3       m_aabbMax;
+
+               int     m_shapeType;
+
+/*             void*   m_userPtr;
+               short int       m_collisionFilterGroup;
+               short int       m_collisionFilterMask;
+*/
+               btMultiSapProxy(const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
+                       :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask),
+                       m_aabbMin(aabbMin),
+                       m_aabbMax(aabbMax),
+                       m_shapeType(shapeType)
+               {
+                       m_multiSapParentProxy =this;
+               }
+
+               
+       };
+
+protected:
+
+
+       btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies;
+
+public:
+
+       btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0);
+
+
+       btSapBroadphaseArray&   getBroadphaseArray()
+       {
+               return m_sapBroadphases;
+       }
+
+       const btSapBroadphaseArray&     getBroadphaseArray() const
+       {
+               return m_sapBroadphases;
+       }
+
+       virtual ~btMultiSapBroadphase();
+
+       virtual btBroadphaseProxy*      createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
+       virtual void    destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+       virtual void    setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
+
+       void    addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*        childBroadphase);
+
+       ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+       virtual void    calculateOverlappingPairs(btDispatcher* dispatcher);
+
+       bool    testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+       virtual btOverlappingPairCache* getOverlappingPairCache()
+       {
+               return m_overlappingPairs;
+       }
+       virtual const btOverlappingPairCache*   getOverlappingPairCache() const
+       {
+               return m_overlappingPairs;
+       }
+
+       ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+       ///will add some transform later
+       virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+       {
+               aabbMin.setValue(-1e30f,-1e30f,-1e30f);
+               aabbMax.setValue(1e30f,1e30f,1e30f);
+       }
+
+       void    buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax);
+
+       virtual void    printStats();
+
+       void quicksort (btBroadphasePairArray& a, int lo, int hi);
+
+};
+
+#endif //BT_MULTI_SAP_BROADPHASE
index 60f0a41a9d76ae9f9ffb203e9dc79b3b80814018..ff65cdde79f209e134becb8942edd369286c29fb 100644 (file)
@@ -1,4 +1,3 @@
-
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
@@ -21,66 +20,415 @@ subject to the following restrictions:
 #include "btDispatcher.h"
 #include "btCollisionAlgorithm.h"
 
+#include <stdio.h>
+
 int    gOverlappingPairs = 0;
 
-btOverlappingPairCache::btOverlappingPairCache():
-m_blockedForChanges(false),
-m_overlapFilterCallback(0)
-//m_NumOverlapBroadphasePair(0)
+int gRemovePairs =0;
+int gAddedPairs =0;
+int gFindPairs =0;
+
+
+
+
+btHashedOverlappingPairCache::btHashedOverlappingPairCache():
+       m_overlapFilterCallback(0),
+       m_blockedForChanges(false)
 {
+       int initialAllocatedSize= 2;
+       m_overlappingPairArray.reserve(initialAllocatedSize);
+       growTables();
 }
 
 
-btOverlappingPairCache::~btOverlappingPairCache()
+
+
+btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
 {
        //todo/test: show we erase/delete data, or is it automatic
 }
 
 
-void   btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair)
+
+void   btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
 {
-       
-       int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
-       if (findIndex < m_overlappingPairArray.size())
+       if (pair.m_algorithm)
+       {
+               {
+                       pair.m_algorithm->~btCollisionAlgorithm();
+                       dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+                       pair.m_algorithm=0;
+               }
+       }
+}
+
+
+
+
+void   btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+
+       class   CleanPairCallback : public btOverlapCallback
+       {
+               btBroadphaseProxy* m_cleanProxy;
+               btOverlappingPairCache* m_pairCache;
+               btDispatcher* m_dispatcher;
+
+       public:
+               CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
+                       :m_cleanProxy(cleanProxy),
+                       m_pairCache(pairCache),
+                       m_dispatcher(dispatcher)
+               {
+               }
+               virtual bool    processOverlap(btBroadphasePair& pair)
+               {
+                       if ((pair.m_pProxy0 == m_cleanProxy) ||
+                               (pair.m_pProxy1 == m_cleanProxy))
+                       {
+                               m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+                       }
+                       return false;
+               }
+               
+       };
+
+       CleanPairCallback cleanPairs(proxy,this,dispatcher);
+
+       processAllOverlappingPairs(&cleanPairs,dispatcher);
+
+}
+
+
+
+
+void   btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+
+       class   RemovePairCallback : public btOverlapCallback
        {
-               gOverlappingPairs--;
-               btBroadphasePair& pair = m_overlappingPairArray[findIndex];
-               cleanOverlappingPair(pair);
+               btBroadphaseProxy* m_obsoleteProxy;
+
+       public:
+               RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
+                       :m_obsoleteProxy(obsoleteProxy)
+               {
+               }
+               virtual bool    processOverlap(btBroadphasePair& pair)
+               {
+                       return ((pair.m_pProxy0 == m_obsoleteProxy) ||
+                               (pair.m_pProxy1 == m_obsoleteProxy));
+               }
                
-               m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1);
+       };
+
+
+       RemovePairCallback removeCallback(proxy);
+
+       processAllOverlappingPairs(&removeCallback,dispatcher);
+}
+
+
+
+
+
+btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+       gFindPairs++;
+       if(proxy0>proxy1) btSwap(proxy0,proxy1);
+       int proxyId1 = proxy0->getUid();
+       int proxyId2 = proxy1->getUid();
+
+       /*if (proxyId1 > proxyId2) 
+               btSwap(proxyId1, proxyId2);*/
+
+       int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+
+       if (hash >= m_hashTable.size())
+       {
+               return NULL;
+       }
+
+       int index = m_hashTable[hash];
+       while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+       {
+               index = m_next[index];
+       }
+
+       if (index == BT_NULL_PAIR)
+       {
+               return NULL;
+       }
+
+       btAssert(index < m_overlappingPairArray.size());
+
+       return &m_overlappingPairArray[index];
+}
+
+//#include <stdio.h>
+
+void   btHashedOverlappingPairCache::growTables()
+{
+
+       int newCapacity = m_overlappingPairArray.capacity();
+
+       if (m_hashTable.size() < newCapacity)
+       {
+               //grow hashtable and next table
+               int curHashtableSize = m_hashTable.size();
+
+               m_hashTable.resize(newCapacity);
+               m_next.resize(newCapacity);
+
+
+               int i;
+
+               for (i= 0; i < newCapacity; ++i)
+               {
+                       m_hashTable[i] = BT_NULL_PAIR;
+               }
+               for (i = 0; i < newCapacity; ++i)
+               {
+                       m_next[i] = BT_NULL_PAIR;
+               }
+
+               for(i=0;i<curHashtableSize;i++)
+               {
+       
+                       const btBroadphasePair& pair = m_overlappingPairArray[i];
+                       int proxyId1 = pair.m_pProxy0->getUid();
+                       int proxyId2 = pair.m_pProxy1->getUid();
+                       /*if (proxyId1 > proxyId2) 
+                               btSwap(proxyId1, proxyId2);*/
+                       int     hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
+                       m_next[i] = m_hashTable[hashValue];
+                       m_hashTable[hashValue] = i;
+               }
+
+
+       }
+}
+
+btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+       if(proxy0>proxy1) btSwap(proxy0,proxy1);
+       int proxyId1 = proxy0->getUid();
+       int proxyId2 = proxy1->getUid();
+
+       /*if (proxyId1 > proxyId2) 
+               btSwap(proxyId1, proxyId2);*/
+
+       int     hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));      // New hash value with new mask
+
+
+       btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+       if (pair != NULL)
+       {
+               return pair;
+       }
+       /*for(int i=0;i<m_overlappingPairArray.size();++i)
+               {
+               if(     (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
+                       (m_overlappingPairArray[i].m_pProxy1==proxy1))
+                       {
+                       printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
+                       internalFindPair(proxy0, proxy1, hash);
+                       }
+               }*/
+       int count = m_overlappingPairArray.size();
+       int oldCapacity = m_overlappingPairArray.capacity();
+       void* mem = &m_overlappingPairArray.expand();
+       int newCapacity = m_overlappingPairArray.capacity();
+
+       if (oldCapacity < newCapacity)
+       {
+               growTables();
+               //hash with new capacity
+               hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+       }
+       
+       pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
+//     pair->m_pProxy0 = proxy0;
+//     pair->m_pProxy1 = proxy1;
+       pair->m_algorithm = 0;
+       pair->m_userInfo = 0;
+       
+
+       m_next[count] = m_hashTable[hash];
+       m_hashTable[hash] = count;
+
+       return pair;
+}
+
+
+
+void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+{
+       gRemovePairs++;
+       if(proxy0>proxy1) btSwap(proxy0,proxy1);
+       int proxyId1 = proxy0->getUid();
+       int proxyId2 = proxy1->getUid();
+
+       /*if (proxyId1 > proxyId2) 
+               btSwap(proxyId1, proxyId2);*/
+
+       int     hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+
+       btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+       if (pair == NULL)
+       {
+               return 0;
+       }
+
+       cleanOverlappingPair(*pair,dispatcher);
+
+       void* userData = pair->m_userInfo;
+
+       btAssert(pair->m_pProxy0->getUid() == proxyId1);
+       btAssert(pair->m_pProxy1->getUid() == proxyId2);
+
+       int pairIndex = int(pair - &m_overlappingPairArray[0]);
+       btAssert(pairIndex < m_overlappingPairArray.size());
+
+       // Remove the pair from the hash table.
+       int index = m_hashTable[hash];
+       btAssert(index != BT_NULL_PAIR);
+
+       int previous = BT_NULL_PAIR;
+       while (index != pairIndex)
+       {
+               previous = index;
+               index = m_next[index];
+       }
+
+       if (previous != BT_NULL_PAIR)
+       {
+               btAssert(m_next[previous] == pairIndex);
+               m_next[previous] = m_next[pairIndex];
+       }
+       else
+       {
+               m_hashTable[hash] = m_next[pairIndex];
+       }
+
+       // We now move the last pair into spot of the
+       // pair being removed. We need to fix the hash
+       // table indices to support the move.
+
+       int lastPairIndex = m_overlappingPairArray.size() - 1;
+
+       // If the removed pair is the last pair, we are done.
+       if (lastPairIndex == pairIndex)
+       {
                m_overlappingPairArray.pop_back();
+               return userData;
+       }
+
+       // Remove the last pair from the hash table.
+       const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
+               /* missing swap here too, Nat. */ 
+       int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1));
+
+       index = m_hashTable[lastHash];
+       btAssert(index != BT_NULL_PAIR);
+
+       previous = BT_NULL_PAIR;
+       while (index != lastPairIndex)
+       {
+               previous = index;
+               index = m_next[index];
+       }
+
+       if (previous != BT_NULL_PAIR)
+       {
+               btAssert(m_next[previous] == lastPairIndex);
+               m_next[previous] = m_next[lastPairIndex];
+       }
+       else
+       {
+               m_hashTable[lastHash] = m_next[lastPairIndex];
+       }
+
+       // Copy the last pair into the remove pair's spot.
+       m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
+
+       // Insert the last pair into the hash table
+       m_next[pairIndex] = m_hashTable[lastHash];
+       m_hashTable[lastHash] = pairIndex;
+
+       m_overlappingPairArray.pop_back();
+
+       return userData;
+}
+//#include <stdio.h>
+
+void   btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+{
+
+       int i;
+
+//     printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
+       for (i=0;i<m_overlappingPairArray.size();)
+       {
+       
+               btBroadphasePair* pair = &m_overlappingPairArray[i];
+               if (callback->processOverlap(*pair))
+               {
+                       removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
+
+                       gOverlappingPairs--;
+               } else
+               {
+                       i++;
+               }
        }
 }
 
 
-void   btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair)
+
+void*  btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
 {
-       if (pair.m_algorithm)
+       if (!hasDeferredRemoval())
        {
+               btBroadphasePair findPair(*proxy0,*proxy1);
+
+               int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
+               if (findIndex < m_overlappingPairArray.size())
                {
-                       delete pair.m_algorithm;;
-                       pair.m_algorithm=0;
+                       gOverlappingPairs--;
+                       btBroadphasePair& pair = m_overlappingPairArray[findIndex];
+                       void* userData = pair.m_userInfo;
+                       cleanOverlappingPair(pair,dispatcher);
+                       
+                       m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
+                       m_overlappingPairArray.pop_back();
+                       return userData;
                }
        }
+
+       return 0;
 }
 
 
 
 
 
-void   btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+
+
+
+btBroadphasePair*      btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 {
        //don't add overlap with own
        assert(proxy0 != proxy1);
 
        if (!needsBroadphaseCollision(proxy0,proxy1))
-               return;
-
-
-       btBroadphasePair pair(*proxy0,*proxy1);
+               return 0;
        
-       m_overlappingPairArray.push_back(pair);
+       void* mem = &m_overlappingPairArray.expand();
+       btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
        gOverlappingPairs++;
+       gAddedPairs++;
+       return pair;
        
 }
 
@@ -88,7 +436,7 @@ void btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa
 ///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
 ///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
 ///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
- btBroadphasePair*     btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+ btBroadphasePair*     btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
 {
        if (!needsBroadphaseCollision(proxy0,proxy1))
                return 0;
@@ -109,18 +457,81 @@ void      btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa
 
 
 
-void   btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy)
+
+
+
+
+
+//#include <stdio.h>
+
+void   btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+{
+
+       int i;
+
+       for (i=0;i<m_overlappingPairArray.size();)
+       {
+       
+               btBroadphasePair* pair = &m_overlappingPairArray[i];
+               if (callback->processOverlap(*pair))
+               {
+                       cleanOverlappingPair(*pair,dispatcher);
+
+                       m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1);
+                       m_overlappingPairArray.pop_back();
+                       gOverlappingPairs--;
+               } else
+               {
+                       i++;
+               }
+       }
+}
+
+
+
+
+btSortedOverlappingPairCache::btSortedOverlappingPairCache():
+       m_blockedForChanges(false),
+       m_hasDeferredRemoval(true),
+       m_overlapFilterCallback(0)
+{
+       int initialAllocatedSize= 2;
+       m_overlappingPairArray.reserve(initialAllocatedSize);
+}
+
+btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
+{
+       //todo/test: show we erase/delete data, or is it automatic
+}
+
+void   btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
+{
+       if (pair.m_algorithm)
+       {
+               {
+                       pair.m_algorithm->~btCollisionAlgorithm();
+                       dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+                       pair.m_algorithm=0;
+                       gRemovePairs--;
+               }
+       }
+}
+
+
+void   btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 
        class   CleanPairCallback : public btOverlapCallback
        {
                btBroadphaseProxy* m_cleanProxy;
                btOverlappingPairCache* m_pairCache;
+               btDispatcher* m_dispatcher;
 
        public:
-               CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache)
+               CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
                        :m_cleanProxy(cleanProxy),
-                       m_pairCache(pairCache)
+                       m_pairCache(pairCache),
+                       m_dispatcher(dispatcher)
                {
                }
                virtual bool    processOverlap(btBroadphasePair& pair)
@@ -128,22 +539,21 @@ void      btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy)
                        if ((pair.m_pProxy0 == m_cleanProxy) ||
                                (pair.m_pProxy1 == m_cleanProxy))
                        {
-                               m_pairCache->cleanOverlappingPair(pair);
+                               m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
                        }
                        return false;
                }
                
        };
 
-       CleanPairCallback cleanPairs(proxy,this);
+       CleanPairCallback cleanPairs(proxy,this,dispatcher);
 
-       processAllOverlappingPairs(&cleanPairs);
+       processAllOverlappingPairs(&cleanPairs,dispatcher);
 
 }
 
 
-
-void   btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy)
+void   btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
 {
 
        class   RemovePairCallback : public btOverlapCallback
@@