From 982a5cc60dd93b21ce110b3fff831d05720047bf Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 3 Mar 2009 03:41:24 +0000 Subject: Upgraded to Bullet 2.74. The upgrade introduced a few bugs, which need to be fixed before Blender 2.49. In particular, the Bullet vehicle seems broken, and some soft-body demos don't work. No new features or benefits are added yet, but a few improvements are planned before Blender 2.49 release. Please update the build systems, and add those 3 files: extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp I'll watch the Blender mailing list, in case this commit causes some issues. --- extern/bullet2/src/Bullet-C-Api.h | 18 +- .../BroadphaseCollision/btAxisSweep3.cpp | 9 +- .../BroadphaseCollision/btAxisSweep3.h | 180 +- .../BroadphaseCollision/btBroadphaseInterface.h | 22 +- .../BroadphaseCollision/btBroadphaseProxy.h | 45 +- .../BulletCollision/BroadphaseCollision/btDbvt.cpp | 1512 +++++++-------- .../BulletCollision/BroadphaseCollision/btDbvt.h | 1297 +++++++------ .../BroadphaseCollision/btDbvtBroadphase.cpp | 755 +++++--- .../BroadphaseCollision/btDbvtBroadphase.h | 116 +- .../BroadphaseCollision/btDispatcher.h | 17 +- .../BroadphaseCollision/btMultiSapBroadphase.cpp | 25 +- .../BroadphaseCollision/btMultiSapBroadphase.h | 9 +- .../BroadphaseCollision/btOverlappingPairCache.cpp | 82 +- .../BroadphaseCollision/btOverlappingPairCache.h | 37 +- .../BroadphaseCollision/btQuantizedBvh.cpp | 193 +- .../BroadphaseCollision/btQuantizedBvh.h | 69 +- .../BroadphaseCollision/btSimpleBroadphase.cpp | 58 +- .../BroadphaseCollision/btSimpleBroadphase.h | 31 +- extern/bullet2/src/BulletCollision/CMakeLists.txt | 383 ++-- .../CollisionDispatch/SphereTriangleDetector.cpp | 18 +- .../CollisionDispatch/SphereTriangleDetector.h | 8 +- .../btActivatingCollisionAlgorithm.cpp | 47 + .../btActivatingCollisionAlgorithm.h | 36 + .../btBoxBoxCollisionAlgorithm.cpp | 2 +- .../CollisionDispatch/btBoxBoxCollisionAlgorithm.h | 6 +- .../CollisionDispatch/btBoxBoxDetector.cpp | 14 +- .../CollisionDispatch/btCollisionConfiguration.h | 2 +- .../CollisionDispatch/btCollisionCreateFunc.h | 1 - .../CollisionDispatch/btCollisionDispatcher.cpp | 19 +- .../CollisionDispatch/btCollisionObject.cpp | 1 + .../CollisionDispatch/btCollisionObject.h | 44 +- .../CollisionDispatch/btCollisionWorld.cpp | 289 ++- .../CollisionDispatch/btCollisionWorld.h | 47 +- .../btCompoundCollisionAlgorithm.cpp | 69 +- .../btCompoundCollisionAlgorithm.h | 11 +- .../btConvexConcaveCollisionAlgorithm.cpp | 6 +- .../btConvexConcaveCollisionAlgorithm.h | 7 +- .../CollisionDispatch/btConvexConvexAlgorithm.cpp | 204 +- .../CollisionDispatch/btConvexConvexAlgorithm.h | 39 +- .../btConvexPlaneCollisionAlgorithm.cpp | 85 +- .../btConvexPlaneCollisionAlgorithm.h | 29 +- .../btDefaultCollisionConfiguration.cpp | 12 +- .../btDefaultCollisionConfiguration.h | 16 +- .../CollisionDispatch/btGhostObject.cpp | 171 ++ .../CollisionDispatch/btGhostObject.h | 174 ++ .../CollisionDispatch/btManifoldResult.cpp | 6 +- .../CollisionDispatch/btManifoldResult.h | 3 + .../btSimulationIslandManager.cpp | 166 +- .../CollisionDispatch/btSimulationIslandManager.h | 16 +- .../btSphereBoxCollisionAlgorithm.cpp | 2 +- .../btSphereBoxCollisionAlgorithm.h | 4 +- .../btSphereSphereCollisionAlgorithm.cpp | 4 +- .../btSphereSphereCollisionAlgorithm.h | 6 +- .../btSphereTriangleCollisionAlgorithm.cpp | 6 +- .../btSphereTriangleCollisionAlgorithm.h | 6 +- .../CollisionDispatch/btUnionFind.cpp | 2 - .../CollisionDispatch/btUnionFind.h | 8 +- .../BulletCollision/CollisionShapes/btBoxShape.h | 36 +- .../CollisionShapes/btBvhTriangleMeshShape.cpp | 58 +- .../CollisionShapes/btBvhTriangleMeshShape.h | 7 +- .../CollisionShapes/btCapsuleShape.cpp | 3 +- .../CollisionShapes/btCapsuleShape.h | 6 +- .../CollisionShapes/btCollisionShape.cpp | 10 +- .../CollisionShapes/btCollisionShape.h | 15 +- .../CollisionShapes/btCompoundShape.cpp | 65 +- .../CollisionShapes/btCompoundShape.h | 26 +- .../CollisionShapes/btConcaveShape.h | 10 + .../CollisionShapes/btConeShape.cpp | 6 +- .../BulletCollision/CollisionShapes/btConeShape.h | 3 - .../CollisionShapes/btConvexHullShape.cpp | 45 +- .../CollisionShapes/btConvexHullShape.h | 42 +- .../CollisionShapes/btConvexInternalShape.cpp | 5 +- .../CollisionShapes/btConvexInternalShape.h | 11 +- .../CollisionShapes/btConvexPointCloudShape.cpp | 156 ++ .../CollisionShapes/btConvexPointCloudShape.h | 96 + .../CollisionShapes/btConvexShape.cpp | 361 ++++ .../CollisionShapes/btConvexShape.h | 26 +- .../CollisionShapes/btConvexTriangleMeshShape.cpp | 18 +- .../CollisionShapes/btConvexTriangleMeshShape.h | 10 +- .../CollisionShapes/btCylinderShape.cpp | 2 +- .../CollisionShapes/btCylinderShape.h | 6 +- .../CollisionShapes/btEmptyShape.cpp | 3 +- .../BulletCollision/CollisionShapes/btEmptyShape.h | 6 +- .../CollisionShapes/btHeightfieldTerrainShape.cpp | 251 ++- .../CollisionShapes/btHeightfieldTerrainShape.h | 108 +- .../CollisionShapes/btMinkowskiSumShape.cpp | 10 +- .../CollisionShapes/btMinkowskiSumShape.h | 2 - .../CollisionShapes/btMultiSphereShape.cpp | 3 +- .../CollisionShapes/btMultiSphereShape.h | 1 - .../btMultimaterialTriangleMeshShape.h | 11 +- .../CollisionShapes/btOptimizedBvh.cpp | 26 +- .../CollisionShapes/btPolyhedralConvexShape.cpp | 5 +- .../CollisionShapes/btPolyhedralConvexShape.h | 25 +- .../btScaledBvhTriangleMeshShape.cpp | 11 +- .../CollisionShapes/btScaledBvhTriangleMeshShape.h | 7 +- .../CollisionShapes/btSphereShape.cpp | 6 - .../CollisionShapes/btSphereShape.h | 9 +- .../CollisionShapes/btStaticPlaneShape.cpp | 3 +- .../CollisionShapes/btStaticPlaneShape.h | 5 - .../CollisionShapes/btStridingMeshInterface.h | 23 +- .../CollisionShapes/btTetrahedronShape.cpp | 35 +- .../CollisionShapes/btTetrahedronShape.h | 21 +- .../CollisionShapes/btTriangleBuffer.h | 2 +- .../CollisionShapes/btTriangleIndexVertexArray.cpp | 4 +- .../CollisionShapes/btTriangleIndexVertexArray.h | 8 +- .../btTriangleIndexVertexMaterialArray.cpp | 1 + .../CollisionShapes/btTriangleMesh.cpp | 52 +- .../CollisionShapes/btTriangleMesh.h | 8 +- .../CollisionShapes/btTriangleMeshShape.cpp | 5 +- .../CollisionShapes/btTriangleMeshShape.h | 2 +- .../CollisionShapes/btTriangleShape.h | 17 +- .../CollisionShapes/btUniformScalingShape.cpp | 3 +- .../CollisionShapes/btUniformScalingShape.h | 1 - .../src/BulletCollision/Gimpact/btBoxCollision.h | 8 +- .../src/BulletCollision/Gimpact/btClipPolygon.h | 4 - .../Gimpact/btContactProcessing.cpp | 6 +- .../BulletCollision/Gimpact/btContactProcessing.h | 24 +- .../src/BulletCollision/Gimpact/btGImpactBvh.cpp | 15 +- .../src/BulletCollision/Gimpact/btGImpactBvh.h | 47 +- .../Gimpact/btGImpactCollisionAlgorithm.cpp | 13 +- .../Gimpact/btGImpactCollisionAlgorithm.h | 6 +- .../Gimpact/btGImpactQuantizedBvh.cpp | 17 +- .../Gimpact/btGImpactQuantizedBvh.h | 21 +- .../src/BulletCollision/Gimpact/btGImpactShape.h | 17 +- .../Gimpact/btGenericPoolAllocator.cpp | 6 +- .../Gimpact/btGenericPoolAllocator.h | 5 - .../BulletCollision/Gimpact/btGeometryOperations.h | 4 - .../src/BulletCollision/Gimpact/btQuantization.h | 4 - .../BulletCollision/Gimpact/btTriangleShapeEx.cpp | 8 +- .../BulletCollision/Gimpact/btTriangleShapeEx.h | 10 +- .../src/BulletCollision/Gimpact/gim_array.h | 8 +- .../Gimpact/gim_basic_geometry_operations.h | 4 - .../src/BulletCollision/Gimpact/gim_bitset.h | 6 - .../BulletCollision/Gimpact/gim_box_collision.h | 4 - .../src/BulletCollision/Gimpact/gim_box_set.cpp | 4 +- .../src/BulletCollision/Gimpact/gim_box_set.h | 7 - .../src/BulletCollision/Gimpact/gim_clip_polygon.h | 4 - .../src/BulletCollision/Gimpact/gim_contact.h | 16 +- .../src/BulletCollision/Gimpact/gim_geom_types.h | 7 +- .../src/BulletCollision/Gimpact/gim_hash_table.h | 26 +- .../src/BulletCollision/Gimpact/gim_linear_math.h | 11 - .../bullet2/src/BulletCollision/Gimpact/gim_math.h | 32 +- .../src/BulletCollision/Gimpact/gim_memory.h | 47 +- .../src/BulletCollision/Gimpact/gim_radixsort.h | 8 +- .../BulletCollision/Gimpact/gim_tri_collision.cpp | 6 +- .../BulletCollision/Gimpact/gim_tri_collision.h | 6 +- .../btContinuousConvexCollision.cpp | 8 +- .../btConvexPenetrationDepthSolver.h | 3 +- .../NarrowPhaseCollision/btGjkEpa2.cpp | 1420 +++++++------- .../btGjkEpaPenetrationDepthSolver.cpp | 13 +- .../btGjkEpaPenetrationDepthSolver.h | 2 +- .../NarrowPhaseCollision/btGjkPairDetector.cpp | 70 +- .../NarrowPhaseCollision/btGjkPairDetector.h | 11 +- .../NarrowPhaseCollision/btManifoldPoint.h | 8 + .../btMinkowskiPenetrationDepthSolver.cpp | 19 +- .../btMinkowskiPenetrationDepthSolver.h | 3 +- .../NarrowPhaseCollision/btPersistentManifold.cpp | 11 +- .../NarrowPhaseCollision/btPersistentManifold.h | 20 +- .../NarrowPhaseCollision/btRaycastCallback.cpp | 20 +- .../NarrowPhaseCollision/btRaycastCallback.h | 13 +- .../btSimplexSolverInterface.h | 7 +- .../btVoronoiSimplexSolver.cpp | 24 +- .../NarrowPhaseCollision/btVoronoiSimplexSolver.h | 22 +- extern/bullet2/src/BulletDynamics/CMakeLists.txt | 93 +- .../ConstraintSolver/btConeTwistConstraint.cpp | 948 +++++++-- .../ConstraintSolver/btConeTwistConstraint.h | 109 +- .../ConstraintSolver/btContactConstraint.cpp | 14 +- .../ConstraintSolver/btContactConstraint.h | 2 +- .../ConstraintSolver/btContactSolverInfo.h | 16 +- .../ConstraintSolver/btGeneric6DofConstraint.cpp | 865 ++++++--- .../ConstraintSolver/btGeneric6DofConstraint.h | 55 +- .../ConstraintSolver/btHingeConstraint.cpp | 667 +++++-- .../ConstraintSolver/btHingeConstraint.h | 30 +- .../ConstraintSolver/btPoint2PointConstraint.cpp | 207 +- .../ConstraintSolver/btPoint2PointConstraint.h | 9 +- .../btSequentialImpulseConstraintSolver.cpp | 1779 +++++++---------- .../btSequentialImpulseConstraintSolver.h | 75 +- .../ConstraintSolver/btSliderConstraint.cpp | 462 ++++- .../ConstraintSolver/btSliderConstraint.h | 15 +- .../BulletDynamics/ConstraintSolver/btSolverBody.h | 160 +- .../ConstraintSolver/btSolverConstraint.h | 62 +- .../ConstraintSolver/btTypedConstraint.cpp | 64 +- .../ConstraintSolver/btTypedConstraint.h | 65 +- .../src/BulletDynamics/Dynamics/Bullet-C-API.cpp | 22 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 370 +++- .../Dynamics/btDiscreteDynamicsWorld.h | 37 +- .../src/BulletDynamics/Dynamics/btDynamicsWorld.h | 8 + .../src/BulletDynamics/Dynamics/btRigidBody.cpp | 12 +- .../src/BulletDynamics/Dynamics/btRigidBody.h | 33 +- .../Dynamics/btSimpleDynamicsWorld.cpp | 6 +- .../Dynamics/btSimpleDynamicsWorld.h | 4 +- .../BulletDynamics/Vehicle/btRaycastVehicle.cpp | 12 +- .../src/BulletDynamics/Vehicle/btRaycastVehicle.h | 13 +- extern/bullet2/src/BulletSoftBody/CMakeLists.txt | 36 +- extern/bullet2/src/BulletSoftBody/btSoftBody.cpp | 2043 ++++++++++---------- extern/bullet2/src/BulletSoftBody/btSoftBody.h | 291 +-- .../btSoftBodyConcaveCollisionAlgorithm.cpp | 120 +- .../btSoftBodyConcaveCollisionAlgorithm.h | 12 +- .../src/BulletSoftBody/btSoftBodyHelpers.cpp | 478 +++-- .../bullet2/src/BulletSoftBody/btSoftBodyHelpers.h | 86 +- .../src/BulletSoftBody/btSoftBodyInternals.h | 747 +++---- .../btSoftBodyRigidBodyCollisionConfiguration.cpp | 18 +- .../btSoftBodyRigidBodyCollisionConfiguration.h | 2 +- .../btSoftRigidCollisionAlgorithm.cpp | 6 +- .../BulletSoftBody/btSoftRigidCollisionAlgorithm.h | 8 +- .../BulletSoftBody/btSoftRigidDynamicsWorld.cpp | 41 +- .../src/BulletSoftBody/btSoftRigidDynamicsWorld.h | 18 +- .../BulletSoftBody/btSoftSoftCollisionAlgorithm.h | 2 +- extern/bullet2/src/BulletSoftBody/btSparseSDF.h | 178 +- extern/bullet2/src/LinearMath/CMakeLists.txt | 28 +- extern/bullet2/src/LinearMath/btAabbUtil2.h | 52 +- .../bullet2/src/LinearMath/btAlignedAllocator.cpp | 30 +- extern/bullet2/src/LinearMath/btAlignedAllocator.h | 7 +- .../bullet2/src/LinearMath/btAlignedObjectArray.h | 26 +- extern/bullet2/src/LinearMath/btConvexHull.cpp | 54 +- extern/bullet2/src/LinearMath/btConvexHull.h | 20 +- extern/bullet2/src/LinearMath/btIDebugDraw.h | 181 ++ extern/bullet2/src/LinearMath/btMatrix3x3.h | 238 ++- extern/bullet2/src/LinearMath/btQuadWord.h | 196 +- extern/bullet2/src/LinearMath/btQuaternion.h | 164 +- extern/bullet2/src/LinearMath/btQuickprof.cpp | 66 +- extern/bullet2/src/LinearMath/btQuickprof.h | 20 +- extern/bullet2/src/LinearMath/btScalar.h | 32 +- extern/bullet2/src/LinearMath/btStackAlloc.h | 1 + extern/bullet2/src/LinearMath/btTransform.h | 66 +- extern/bullet2/src/LinearMath/btTransformUtil.h | 123 +- extern/bullet2/src/LinearMath/btVector3.h | 341 +++- 227 files changed, 14060 insertions(+), 8111 deletions(-) create mode 100644 extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp create mode 100644 extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h create mode 100644 extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp create mode 100644 extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h create mode 100644 extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp create mode 100644 extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h (limited to 'extern') diff --git a/extern/bullet2/src/Bullet-C-Api.h b/extern/bullet2/src/Bullet-C-Api.h index 8074aed3038..a4a8a70524e 100644 --- a/extern/bullet2/src/Bullet-C-Api.h +++ b/extern/bullet2/src/Bullet-C-Api.h @@ -38,37 +38,37 @@ typedef plReal plQuaternion[4]; extern "C" { #endif -/* Particular physics SDK */ +/** Particular physics SDK (C-API) */ PL_DECLARE_HANDLE(plPhysicsSdkHandle); -/* Dynamics world, belonging to some physics SDK */ +/** Dynamics world, belonging to some physics SDK (C-API)*/ PL_DECLARE_HANDLE(plDynamicsWorldHandle); -/* Rigid Body that can be part of a Dynamics World */ +/** Rigid Body that can be part of a Dynamics World (C-API)*/ PL_DECLARE_HANDLE(plRigidBodyHandle); -/* Collision Shape/Geometry, property of a Rigid Body */ +/** Collision Shape/Geometry, property of a Rigid Body (C-API)*/ PL_DECLARE_HANDLE(plCollisionShapeHandle); -/* Constraint for Rigid Bodies */ +/** Constraint for Rigid Bodies (C-API)*/ PL_DECLARE_HANDLE(plConstraintHandle); -/* Triangle Mesh interface */ +/** Triangle Mesh interface (C-API)*/ PL_DECLARE_HANDLE(plMeshInterfaceHandle); -/* Broadphase Scene/Proxy Handles */ +/** Broadphase Scene/Proxy Handles (C-API)*/ 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 */ +/** 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); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp index d7eea33ea41..77763305b1b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp @@ -19,10 +19,9 @@ // 3. This notice may not be removed or altered from any source distribution. #include "btAxisSweep3.h" -#include -btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache) -:btAxisSweep3Internal(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache) +btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator) +:btAxisSweep3Internal(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator) { // 1 handle is reserved as sentinel btAssert(maxHandles > 1 && maxHandles < 32767); @@ -30,8 +29,8 @@ btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAab } -bt32BitAxisSweep3::bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache ) -:btAxisSweep3Internal(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache) +bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache , bool disableRaycastAccelerator) +:btAxisSweep3Internal(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache,disableRaycastAccelerator) { // 1 handle is reserved as sentinel btAssert(maxHandles > 1 && maxHandles < 2147483647); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index d0ad09a385a..cad21b4cad2 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -19,12 +19,12 @@ #ifndef AXIS_SWEEP_3_H #define AXIS_SWEEP_3_H -#include "LinearMath/btPoint3.h" #include "LinearMath/btVector3.h" #include "btOverlappingPairCache.h" #include "btBroadphaseInterface.h" #include "btBroadphaseProxy.h" #include "btOverlappingPairCallback.h" +#include "btDbvtBroadphase.h" //#define DEBUG_BROADPHASE 1 #define USE_OVERLAP_TEST_ON_REMOVES 1 @@ -42,6 +42,7 @@ protected: public: + BT_DECLARE_ALIGNED_ALLOCATOR(); class Edge { @@ -61,8 +62,7 @@ public: // indexes into the edge arrays BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 // BP_FP_INT_TYPE m_uniqueId; - BP_FP_INT_TYPE m_pad; - + btBroadphaseProxy* m_dbvtProxy;//for faster raycast //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;} @@ -71,8 +71,8 @@ public: protected: - btPoint3 m_worldAabbMin; // overall system bounds - btPoint3 m_worldAabbMax; // overall system bounds + btVector3 m_worldAabbMin; // overall system bounds + btVector3 m_worldAabbMax; // overall system bounds btVector3 m_quantize; // scaling factor for quantization @@ -94,6 +94,12 @@ protected: int m_invalidPair; + ///additional dynamic aabb structure, used to accelerate ray cast queries. + ///can be disabled using a optional argument in the constructor + btDbvtBroadphase* m_raycastAccelerator; + btOverlappingPairCache* m_nullPairCache; + + // allocation/deallocation BP_FP_INT_TYPE allocHandle(); void freeHandle(BP_FP_INT_TYPE handle); @@ -108,7 +114,7 @@ protected: //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); - void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const; + 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 ); @@ -117,7 +123,7 @@ protected: public: - 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); + btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false); virtual ~btAxisSweep3Internal(); @@ -128,17 +134,26 @@ public: virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); + BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& 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); + void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;} + virtual void resetPool(btDispatcher* dispatcher); + void processAllOverlappingPairs(btOverlapCallback* callback); //Broadphase Interface 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); + virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); + + void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const; + ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result + void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); @@ -206,7 +221,7 @@ void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinalit } if (checkCardinality) - assert(numEdges == m_numHandles*2+1); + btAssert(numEdges == m_numHandles*2+1); } #endif //DEBUG_BROADPHASE @@ -217,7 +232,12 @@ btBroadphaseProxy* btAxisSweep3Internal::createProxy( const btV BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy); Handle* handle = getHandle(handleId); - + + if (m_raycastAccelerator) + { + btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0); + handle->m_dbvtProxy = rayProxy; + } return handle; } @@ -227,6 +247,8 @@ template void btAxisSweep3Internal::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) { Handle* handle = static_cast(proxy); + if (m_raycastAccelerator) + m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher); removeHandle(static_cast(handle->m_uniqueId), dispatcher); } @@ -234,22 +256,80 @@ template void btAxisSweep3Internal::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) { Handle* handle = static_cast(proxy); + handle->m_aabbMin = aabbMin; + handle->m_aabbMax = aabbMax; updateHandle(static_cast(handle->m_uniqueId), aabbMin, aabbMax,dispatcher); + if (m_raycastAccelerator) + m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher); + +} + +template +void btAxisSweep3Internal::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) +{ + if (m_raycastAccelerator) + { + m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax); + } else + { + //choose axis? + BP_FP_INT_TYPE axis = 0; + //for each proxy + for (BP_FP_INT_TYPE i=1;i +void btAxisSweep3Internal::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + Handle* pHandle = static_cast(proxy); + aabbMin = pHandle->m_aabbMin; + aabbMax = pHandle->m_aabbMax; } +template +void btAxisSweep3Internal::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + Handle* pHandle = static_cast(proxy); + + unsigned short vecInMin[3]; + unsigned short vecInMax[3]; + + vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ; + vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ; + vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ; + vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ; + vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ; + vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ; + + aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ())); + aabbMin += m_worldAabbMin; + + aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ())); + aabbMax += m_worldAabbMin; +} + template -btAxisSweep3Internal::btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache ) +btAxisSweep3Internal::btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator) :m_bpHandleMask(handleMask), m_handleSentinel(handleSentinel), m_pairCache(pairCache), m_userPairCallback(0), m_ownsPairCache(false), -m_invalidPair(0) +m_invalidPair(0), +m_raycastAccelerator(0) { BP_FP_INT_TYPE maxHandles = static_cast(userMaxHandles+1);//need to add one sentinel handle @@ -260,7 +340,14 @@ m_invalidPair(0) m_ownsPairCache = true; } - //assert(bounds.HasVolume()); + if (!disableRaycastAccelerator) + { + m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache(); + m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache); + m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs + } + + //btAssert(bounds.HasVolume()); // init bounds m_worldAabbMin = worldAabbMin; @@ -320,7 +407,14 @@ m_invalidPair(0) template btAxisSweep3Internal::~btAxisSweep3Internal() { - + if (m_raycastAccelerator) + { + m_nullPairCache->~btOverlappingPairCache(); + btAlignedFree(m_nullPairCache); + m_raycastAccelerator->~btDbvtBroadphase(); + btAlignedFree (m_raycastAccelerator); + } + for (int i = 2; i >= 0; i--) { btAlignedFree(m_pEdgesRawPtr[i]); @@ -335,27 +429,31 @@ btAxisSweep3Internal::~btAxisSweep3Internal() } template -void btAxisSweep3Internal::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const +void btAxisSweep3Internal::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const { - btPoint3 clampedPoint(point); - - - +#ifdef OLD_CLAMPING_METHOD + ///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax] + ///see http://code.google.com/p/bullet/issues/detail?id=87 + btVector3 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); - +#else + btVector3 v = (point - m_worldAabbMin) * m_quantize; + out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax); + out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax); + out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax); +#endif //OLD_CLAMPING_METHOD } template BP_FP_INT_TYPE btAxisSweep3Internal::allocHandle() { - assert(m_firstFreeHandle); + btAssert(m_firstFreeHandle); BP_FP_INT_TYPE handle = m_firstFreeHandle; m_firstFreeHandle = getHandle(handle)->GetNextFree(); @@ -367,7 +465,7 @@ BP_FP_INT_TYPE btAxisSweep3Internal::allocHandle() template void btAxisSweep3Internal::freeHandle(BP_FP_INT_TYPE handle) { - assert(handle > 0 && handle < m_maxHandles); + btAssert(handle > 0 && handle < m_maxHandles); getHandle(handle)->SetNextFree(m_firstFreeHandle); m_firstFreeHandle = handle; @@ -377,7 +475,7 @@ void btAxisSweep3Internal::freeHandle(BP_FP_INT_TYPE handle) template -BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) +BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) { // quantize the bounds BP_FP_INT_TYPE min[3], max[3]; @@ -440,7 +538,7 @@ void btAxisSweep3Internal::removeHandle(BP_FP_INT_TYPE handle,bt //explicitly remove the pairs containing the proxy //we could do it also in the sortMinUp (passing true) - //todo: compare performance + ///@todo: compare performance if (!m_pairCache->hasDeferredRemoval()) { m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher); @@ -489,6 +587,21 @@ void btAxisSweep3Internal::removeHandle(BP_FP_INT_TYPE handle,bt } +template +void btAxisSweep3Internal::resetPool(btDispatcher* dispatcher) +{ + if (m_numHandles == 0) + { + m_firstFreeHandle = 1; + { + for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++) + m_pHandles[i].SetNextFree(static_cast(i + 1)); + m_pHandles[m_maxHandles - 1].SetNextFree(0); + } + } +} + + extern int gOverlappingPairs; //#include @@ -529,6 +642,7 @@ void btAxisSweep3Internal::calculateOverlappingPairs(btDispatche if (!isDuplicate) { + ///important to use an AABB test that is consistent with the broadphase bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); if (hasOverlap) @@ -574,10 +688,6 @@ void btAxisSweep3Internal::calculateOverlappingPairs(btDispatche //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); } - - - - } @@ -616,10 +726,10 @@ bool btAxisSweep3Internal::testOverlap2D(const Handle* pHandleA, } template -void btAxisSweep3Internal::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher) +void btAxisSweep3Internal::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) { -// assert(bounds.IsFinite()); - //assert(bounds.HasVolume()); +// btAssert(bounds.IsFinite()); + //btAssert(bounds.HasVolume()); Handle* pHandle = getHandle(handle); @@ -895,7 +1005,7 @@ class btAxisSweep3 : public btAxisSweep3Internal { public: - btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0); + btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); }; @@ -906,7 +1016,7 @@ class bt32BitAxisSweep3 : public btAxisSweep3Internal { public: - bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0); + bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index 200ac365329..b7bbaf512ae 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -21,8 +21,22 @@ subject to the following restrictions: struct btDispatcherInfo; class btDispatcher; #include "btBroadphaseProxy.h" + class btOverlappingPairCache; + + +struct btBroadphaseRayCallback +{ + ///added some cached data to accelerate ray-AABB tests + btVector3 m_rayDirectionInverse; + unsigned int m_signs[3]; + btScalar m_lambda_max; + + virtual ~btBroadphaseRayCallback() {} + virtual bool process(const btBroadphaseProxy* proxy) = 0; +}; + #include "LinearMath/btVector3.h" ///The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs. @@ -36,7 +50,10 @@ public: 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; - + virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0; + + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0; + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb virtual void calculateOverlappingPairs(btDispatcher* dispatcher)=0; @@ -47,6 +64,9 @@ public: ///will add some transform later virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0; + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(btDispatcher* dispatcher) {}; + virtual void printStats() = 0; }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index a074a0b150b..be261ec4080 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -17,20 +17,24 @@ subject to the following restrictions: #define BROADPHASE_PROXY_H #include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE +#include "LinearMath/btVector3.h" #include "LinearMath/btAlignedAllocator.h" /// btDispatcher uses these types /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave /// to facilitate type checking +/// CUSTOM_POLYHEDRAL_SHAPE_TYPE,CUSTOM_CONVEX_SHAPE_TYPE and CUSTOM_CONCAVE_SHAPE_TYPE can be used to extend Bullet without modifying source code enum BroadphaseNativeTypes { -// polyhedral convex shapes + // polyhedral convex shapes BOX_SHAPE_PROXYTYPE, TRIANGLE_SHAPE_PROXYTYPE, TETRAHEDRAL_SHAPE_PROXYTYPE, CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE, CONVEX_HULL_SHAPE_PROXYTYPE, + CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE, + CUSTOM_POLYHEDRAL_SHAPE_TYPE, //implicit convex shapes IMPLICIT_CONVEX_SHAPES_START_HERE, SPHERE_SHAPE_PROXYTYPE, @@ -42,6 +46,7 @@ IMPLICIT_CONVEX_SHAPES_START_HERE, UNIFORM_SCALING_SHAPE_PROXYTYPE, MINKOWSKI_SUM_SHAPE_PROXYTYPE, MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE, + CUSTOM_CONVEX_SHAPE_TYPE, //concave shapes CONCAVE_SHAPES_START_HERE, //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! @@ -58,13 +63,18 @@ CONCAVE_SHAPES_START_HERE, EMPTY_SHAPE_PROXYTYPE, STATIC_PLANE_PROXYTYPE, + CUSTOM_CONCAVE_SHAPE_TYPE, CONCAVE_SHAPES_END_HERE, COMPOUND_SHAPE_PROXYTYPE, SOFTBODY_SHAPE_PROXYTYPE, + HFFLUID_SHAPE_PROXYTYPE, + HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE, + INVALID_SHAPE_PROXYTYPE, MAX_BROADPHASE_COLLISION_TYPES + }; @@ -83,20 +93,20 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); KinematicFilter = 4, DebrisFilter = 8, SensorTrigger = 16, + CharacterFilter = 32, 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. + btVector3 m_aabbMin; + btVector3 m_aabbMax; + SIMD_FORCE_INLINE int getUid() const { return m_uniqueId; @@ -107,10 +117,12 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { } - btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0) + btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0) :m_clientObject(userPtr), m_collisionFilterGroup(collisionFilterGroup), - m_collisionFilterMask(collisionFilterMask) + m_collisionFilterMask(collisionFilterMask), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) { m_multiSapParentProxy = multiSapParentProxy; } @@ -159,7 +171,7 @@ ATTRIBUTE_ALIGNED16(struct) btBroadphasePair m_pProxy0(0), m_pProxy1(0), m_algorithm(0), - m_userInfo(0) + m_internalInfo1(0) { } @@ -169,14 +181,14 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); : m_pProxy0(other.m_pProxy0), m_pProxy1(other.m_pProxy1), m_algorithm(other.m_algorithm), - m_userInfo(other.m_userInfo) + m_internalInfo1(other.m_internalInfo1) { } btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) { //keep them sorted, so the std::set operations work - if (&proxy0 < &proxy1) + if (proxy0.m_uniqueId < proxy1.m_uniqueId) { m_pProxy0 = &proxy0; m_pProxy1 = &proxy1; @@ -188,7 +200,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); } m_algorithm = 0; - m_userInfo = 0; + m_internalInfo1 = 0; } @@ -196,7 +208,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); btBroadphaseProxy* m_pProxy1; mutable btCollisionAlgorithm* m_algorithm; - mutable void* m_userInfo; + union { void* m_internalInfo1; int m_internalTmpValue;};//don't use this data, it will be removed in future version. }; @@ -217,8 +229,13 @@ class btBroadphasePairSortPredicate bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) { - return a.m_pProxy0 > b.m_pProxy0 || - (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) || + const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1; + const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1; + const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1; + const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1; + + return uidA0 > uidB0 || + (a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) || (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); } }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 7c41c8d8f71..a6e36b47049 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -23,188 +23,188 @@ typedef btAlignedObjectArray tConstNodeArray; // struct btDbvtNodeEnumerator : btDbvt::ICollide { -tConstNodeArray nodes; -void Process(const btDbvtNode* n) { nodes.push_back(n); } + 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); + return(node->parent->childs[1]==node); } // static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, - const btDbvtVolume& b) + const btDbvtVolume& b) { -#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE -DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)]; -btDbvtVolume& res=*(btDbvtVolume*)locals; +#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE) + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]); + btDbvtVolume& res=*(btDbvtVolume*)locals; #else -btDbvtVolume res; + btDbvtVolume res; #endif -Merge(a,b,res); -return(res); + 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()+ + 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()) + if(node->isinternal()) { - getmaxdepth(node->childs[0],depth+1,maxdepth); - getmaxdepth(node->childs[0],depth+1,maxdepth); + 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) + btDbvtNode* node) { -btAlignedFree(pdbvt->m_free); -pdbvt->m_free=node; + btAlignedFree(pdbvt->m_free); + pdbvt->m_free=node; } - + // static void recursedeletenode( btDbvt* pdbvt, - btDbvtNode* node) + btDbvtNode* node) { -if(!node->isleaf()) + if(!node->isleaf()) { - recursedeletenode(pdbvt,node->childs[0]); - recursedeletenode(pdbvt,node->childs[1]); + recursedeletenode(pdbvt,node->childs[0]); + recursedeletenode(pdbvt,node->childs[1]); } -if(node==pdbvt->m_root) pdbvt->m_root=0; -deletenode(pdbvt,node); + if(node==pdbvt->m_root) pdbvt->m_root=0; + deletenode(pdbvt,node); } // static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - void* data) + btDbvtNode* parent, + void* data) { -btDbvtNode* node; -if(pdbvt->m_free) + 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); + 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* parent, + const btDbvtVolume& volume, + void* data) { -btDbvtNode* node=createnode(pdbvt,parent,data); -node->volume=volume; -return(node); + 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* parent, + const btDbvtVolume& volume0, + const btDbvtVolume& volume1, + void* data) { -btDbvtNode* node=createnode(pdbvt,parent,data); -Merge(volume0,volume1,node->volume); -return(node); + btDbvtNode* node=createnode(pdbvt,parent,data); + Merge(volume0,volume1,node->volume); + return(node); } // static void insertleaf( btDbvt* pdbvt, - btDbvtNode* root, - btDbvtNode* leaf) + btDbvtNode* root, + btDbvtNode* leaf) { -if(!pdbvt->m_root) + if(!pdbvt->m_root) { - pdbvt->m_root = leaf; - leaf->parent = 0; + pdbvt->m_root = leaf; + leaf->parent = 0; } else { - if(!root->isleaf()) + if(!root->isleaf()) { - do { - root=root->childs[Select( leaf->volume, - root->childs[0]->volume, - root->childs[1]->volume)]; + 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) + 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); + 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; + 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; + 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) + btDbvtNode* leaf) { -if(leaf==pdbvt->m_root) + if(leaf==pdbvt->m_root) { - pdbvt->m_root=0; - return(0); + pdbvt->m_root=0; + return(0); } else { - btDbvtNode* parent=leaf->parent; - btDbvtNode* prev=parent->parent; - btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; - if(prev) + 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) + 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)) + const btDbvtVolume pb=prev->volume; + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + if(NotEqual(pb,prev->volume)) { - prev=prev->parent; + prev=prev->parent; } else break; } - return(prev?prev:pdbvt->m_root); + return(prev?prev:pdbvt->m_root); } else { - pdbvt->m_root=sibling; - sibling->parent=0; - deletenode(pdbvt,parent); - return(pdbvt->m_root); + pdbvt->m_root=sibling; + sibling->parent=0; + deletenode(pdbvt,parent); + return(pdbvt->m_root); } } } @@ -215,33 +215,33 @@ static void fetchleaves(btDbvt* pdbvt, tNodeArray& leaves, int depth=-1) { -if(root->isinternal()&&depth) + if(root->isinternal()&&depth) { - fetchleaves(pdbvt,root->childs[0],leaves,depth-1); - fetchleaves(pdbvt,root->childs[1],leaves,depth-1); - deletenode(pdbvt,root); + fetchleaves(pdbvt,root->childs[0],leaves,depth-1); + fetchleaves(pdbvt,root->childs[1],leaves,depth-1); + deletenode(pdbvt,root); } else { - leaves.push_back(root); + leaves.push_back(root); } } // static void split( const tNodeArray& leaves, - tNodeArray& left, - tNodeArray& right, - const btVector3& org, - const btVector3& axis) + tNodeArray& left, + tNodeArray& right, + const btVector3& org, + const btVector3& axis) { -left.resize(0); -right.resize(0); -for(int i=0,ni=leaves.size();ivolume.Center()-org)<0) - left.push_back(leaves[i]); + if(dot(axis,leaves[i]->volume.Center()-org)<0) + left.push_back(leaves[i]); else - right.push_back(leaves[i]); + right.push_back(leaves[i]); } } @@ -249,49 +249,49 @@ for(int i=0,ni=leaves.size();ivolume; + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); + btDbvtVolume& volume=*(btDbvtVolume*)locals; + volume=leaves[0]->volume; #else -btDbvtVolume volume=leaves[0]->volume; + btDbvtVolume volume=leaves[0]->volume; #endif -for(int i=1,ni=leaves.size();ivolume,volume); + Merge(volume,leaves[i]->volume,volume); } -return(volume); + return(volume); } // static void bottomup( btDbvt* pdbvt, - tNodeArray& leaves) + tNodeArray& leaves) { -while(leaves.size()>1) + while(leaves.size()>1) { - btScalar minsize=SIMD_INFINITY; - int minidx[2]={-1,-1}; - for(int i=0;ivolume,leaves[j]->volume)); - if(szvolume,leaves[j]->volume)); + if(szvolume,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(); + 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(); } } @@ -300,175 +300,181 @@ 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) + 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) + 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;ivolume.Center()-org; - for(int j=0;j<3;++j) + const btVector3 x=leaves[i]->volume.Center()-org; + for(int j=0;j<3;++j) { - ++splitcount[j][dot(x,axis[j])>0?1:0]; + ++splitcount[j][dot(x,axis[j])>0?1:0]; } } - for( i=0;i<3;++i) + for( i=0;i<3;++i) { - if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) + if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) { - const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); - if(midp=0) + if(bestaxis>=0) { - sets[0].reserve(splitcount[bestaxis][0]); - sets[1].reserve(splitcount[bestaxis][1]); - split(leaves,sets[0],sets[1],org,axis[bestaxis]); + 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();ichilds[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); + 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]); + bottomup(pdbvt,leaves); + return(leaves[0]); } } -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) + 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); + 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); + return(n); } -// +#if 0 static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) { -while(n&&(count--)) n=n->parent; -return(n); + while(n&&(count--)) n=n->parent; + return(n); } +#endif // // Api // // - btDbvt::btDbvt() +btDbvt::btDbvt() { -m_root = 0; -m_free = 0; -m_lkhd = -1; -m_leaves = 0; -m_opath = 0; + m_root = 0; + m_free = 0; + m_lkhd = -1; + m_leaves = 0; + m_opath = 0; } // - btDbvt::~btDbvt() +btDbvt::~btDbvt() { -clear(); + clear(); } // void btDbvt::clear() { -if(m_root) recursedeletenode(this,m_root); -btAlignedFree(m_free); -m_free=0; + if(m_root) + recursedeletenode(this,m_root); + btAlignedFree(m_free); + m_free=0; + m_lkhd = -1; + m_stkStack.clear(); + m_opath = 0; + } // void btDbvt::optimizeBottomUp() { -if(m_root) + if(m_root) { - tNodeArray leaves; - leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - bottomup(this,leaves); - m_root=leaves[0]; + 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) + if(m_root) { - tNodeArray leaves; - leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - m_root=topdown(this,leaves,bu_treshold); + 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)) + if(passes<0) passes=m_leaves; + if(m_root&&(passes>0)) { - do { - btDbvtNode* node=m_root; - unsigned bit=0; - while(node->isinternal()) + 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); + node=sort(node,m_root)->childs[(m_opath>>bit)&1]; + bit=(bit+1)&(sizeof(unsigned)*8-1); } - update(node); - ++m_opath; + update(node); + ++m_opath; } while(--passes); } } @@ -476,104 +482,104 @@ if(m_root&&(passes>0)) // btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) { -btDbvtNode* leaf=createnode(this,0,volume,data); -insertleaf(this,m_root,leaf); -++m_leaves; -return(leaf); + 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) + btDbvtNode* root=removeleaf(this,leaf); + if(root) { - if(lookahead>=0) + if(lookahead>=0) { - for(int i=0;(iparent;++i) + for(int i=0;(iparent;++i) { - root=root->parent; + root=root->parent; } } else root=m_root; } -insertleaf(this,root,leaf); + insertleaf(this,root,leaf); } // -void btDbvt::update(btDbvtNode* leaf,const btDbvtVolume& volume) +void btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume) { -btDbvtNode* root=removeleaf(this,leaf); -if(root) + btDbvtNode* root=removeleaf(this,leaf); + if(root) { - if(m_lkhd>=0) + if(m_lkhd>=0) { - for(int i=0;(iparent;++i) + for(int i=0;(iparent;++i) { - root=root->parent; + root=root->parent; } } else root=m_root; } -leaf->volume=volume; -insertleaf(this,root,leaf); + leaf->volume=volume; + insertleaf(this,root,leaf); } // -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin) +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); + 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) +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); + if(leaf->volume.Contain(volume)) return(false); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); } // -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin) +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); + 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; + 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;iPrepare(m_root,nodes.nodes.size()); + for(int i=0;iparent) p=nodes.nodes.findLinearSearch(n->parent); - if(n->isinternal()) + 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); + 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); + iwriter->WriteLeaf(n,i,p); } } } @@ -581,29 +587,29 @@ for(int i=0;i 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; + btAlignedObjectArray 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()) + 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)); + stack.push_back(sStkCLN(e.node->childs[0],n)); + stack.push_back(sStkCLN(e.node->childs[1],n)); } else { - iclone->CloneLeaf(n); + iclone->CloneLeaf(n); } } while(stack.size()>0); } @@ -612,31 +618,31 @@ if(m_root!=0) // int btDbvt::maxdepth(const btDbvtNode* node) { -int depth=0; -if(node) getmaxdepth(node,1,depth); -return(depth); + 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])); + if(node->isinternal()) + return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); else - return(1); + return(1); } // void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves) { -if(node->isinternal()) + if(node->isinternal()) { - extractLeaves(node->childs[0],leaves); - extractLeaves(node->childs[1],leaves); + extractLeaves(node->childs[0],leaves); + extractLeaves(node->childs[1],leaves); } else { - leaves.push_back(node); + leaves.push_back(node); } } @@ -657,19 +663,19 @@ q6600,2.4ghz /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 +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: 3499 ms (-1%) [2] btDbvtVolume merges: 1934 ms (0%) [3] btDbvt::collideTT: 5485 ms (-21%) [4] btDbvt::collideTT self: 2814 ms (-20%) [5] btDbvt::collideTT xform: 7379 ms (-1%) [6] btDbvt::collideTT xform,self: 7270 ms (-2%) -[7] btDbvt::collideRAY: 6314 ms (0%),(332143 r/s) +[7] btDbvt::rayTest: 6314 ms (0%),(332143 r/s) [8] insert/remove: 2093 ms (0%),(1001983 ir/s) [9] updates (teleport): 1879 ms (-3%),(1116100 u/s) [10] updates (jitter): 1244 ms (-4%),(1685813 u/s) @@ -684,606 +690,606 @@ Benchmarking dbvt... struct btDbvtBenchmark { -struct NilPolicy : btDbvt::ICollide + 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) + 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) + ++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; + int m_pcount; + btScalar m_depth; + bool m_checksort; }; -struct P14 : btDbvt::ICollide + struct P14 : btDbvt::ICollide { - struct Node + struct Node { - const btDbvtNode* leaf; - btScalar depth; + const btDbvtNode* leaf; + btScalar depth; }; - void Process(const btDbvtNode* leaf,btScalar depth) + void Process(const btDbvtNode* leaf,btScalar depth) { - Node n; - n.leaf = leaf; - n.depth = depth; + Node n; + n.leaf = leaf; + n.depth = depth; } - static int sortfnc(const Node& a,const Node& b) + static int sortfnc(const Node& a,const Node& b) { - if(a.depthb.depth) return(-1); - return(0); + if(a.depthb.depth) return(-1); + return(0); } - btAlignedObjectArray m_nodes; + btAlignedObjectArray m_nodes; }; -struct P15 : btDbvt::ICollide + struct P15 : btDbvt::ICollide { - struct Node + struct Node { - const btDbvtNode* leaf; - btScalar depth; + const btDbvtNode* leaf; + btScalar depth; }; - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - Node n; - n.leaf = leaf; - n.depth = dot(leaf->volume.Center(),m_axis); + Node n; + n.leaf = leaf; + n.depth = dot(leaf->volume.Center(),m_axis); } - static int sortfnc(const Node& a,const Node& b) + static int sortfnc(const Node& a,const Node& b) { - if(a.depthb.depth) return(-1); - return(0); + if(a.depthb.depth) return(-1); + return(0); } - btAlignedObjectArray m_nodes; - btVector3 m_axis; + btAlignedObjectArray m_nodes; + btVector3 m_axis; }; -static btScalar RandUnit() + static btScalar RandUnit() { - return(rand()/(btScalar)RAND_MAX); + return(rand()/(btScalar)RAND_MAX); } -static btVector3 RandVector3() + static btVector3 RandVector3() { - return(btVector3(RandUnit(),RandUnit(),RandUnit())); + return(btVector3(RandUnit(),RandUnit(),RandUnit())); } -static btVector3 RandVector3(btScalar cs) + static btVector3 RandVector3(btScalar cs) { - return(RandVector3()*cs-btVector3(cs,cs,cs)/2); + return(RandVector3()*cs-btVector3(cs,cs,cs)/2); } -static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) + static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) { - return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); + return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); } -static btTransform RandTransform(btScalar cs) + 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); + 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) + static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt) { - dbvt.clear(); - for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i transforms; - btDbvtBenchmark::NilPolicy policy; - transforms.resize(cfgBenchmark5_Iterations); - for(int i=0;i transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark5_Iterations); + for(int i=0;i transforms; - btDbvtBenchmark::NilPolicy policy; - transforms.resize(cfgBenchmark6_Iterations); - for(int i=0;i transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark6_Iterations); + for(int i=0;i rayorg; - btAlignedObjectArray raydir; - btDbvtBenchmark::NilPolicy policy; - rayorg.resize(cfgBenchmark7_Iterations); - raydir.resize(cfgBenchmark7_Iterations); - for(int i=0;i rayorg; + btAlignedObjectArray raydir; + btDbvtBenchmark::NilPolicy policy; + rayorg.resize(cfgBenchmark7_Iterations); + raydir.resize(cfgBenchmark7_Iterations); + for(int i=0;i 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 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(leaves[rand()%cfgLeaves]), - btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); + dbvt.update(const_cast(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); + 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) + if(cfgBenchmark10_Enable) {// Benchmark 10 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray leaves; - btAlignedObjectArray vectors; - vectors.resize(cfgBenchmark10_Iterations); - for(int i=0;i leaves; + btAlignedObjectArray vectors; + vectors.resize(cfgBenchmark10_Iterations); + for(int i=0;i(leaves[rand()%cfgLeaves]); - btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); - dbvt.update(l,v); + const btVector3& d=vectors[j]; + btDbvtNode* l=const_cast(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); + 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) + 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 volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i vectors; - btDbvtBenchmark::NilPolicy policy; - vectors.resize(cfgBenchmark13_Iterations); - for(int i=0;i vectors; + btDbvtBenchmark::NilPolicy policy; + vectors.resize(cfgBenchmark13_Iterations); + for(int i=0;i vectors; - btDbvtBenchmark::P14 policy; - vectors.resize(cfgBenchmark14_Iterations); - for(int i=0;i vectors; + btDbvtBenchmark::P14 policy; + vectors.resize(cfgBenchmark14_Iterations); + for(int i=0;i vectors; - btDbvtBenchmark::P15 policy; - vectors.resize(cfgBenchmark15_Iterations); - for(int i=0;i vectors; + btDbvtBenchmark::P15 policy; + vectors.resize(cfgBenchmark15_Iterations); + for(int i=0;i 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 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 volumes; - btAlignedObjectArray results; - btAlignedObjectArray indices; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - indices.resize(cfgLeaves); - for(int i=0;i volumes; + btAlignedObjectArray results; + btAlignedObjectArray indices; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + indices.resize(cfgLeaves); + for(int i=0;i= 1400) - #define DBVT_USE_TEMPLATE 1 - #else - #define DBVT_USE_TEMPLATE 0 +#ifdef WIN32 +#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 @@ -52,16 +53,11 @@ subject to the following restrictions: // 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_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds +//SSE gives errors on a MSVC 7.1 +#ifdef BT_USE_SSE #define DBVT_SELECT_IMPL DBVT_IMPL_SSE #define DBVT_MERGE_IMPL DBVT_IMPL_SSE #define DBVT_INT0_IMPL DBVT_IMPL_SSE @@ -86,7 +82,7 @@ subject to the following restrictions: #define DBVT_VIRTUAL_DTOR(a) #define DBVT_PREFIX template #define DBVT_IPOLICY T& policy -#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)0; +#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker; #else #define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} #define DBVT_VIRTUAL virtual @@ -133,46 +129,41 @@ subject to the following restrictions: /* 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); + 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 btVector3& b); + + 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; + DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; private: -btVector3 mi,mx; + btVector3 mi,mx; }; // Types @@ -185,88 +176,94 @@ struct btDbvtNode btDbvtNode* parent; DBVT_INLINE bool isleaf() const { return(childs[1]==0); } DBVT_INLINE bool isinternal() const { return(!isleaf()); } - union { - btDbvtNode* childs[2]; - void* data; - int dataAsInt; - }; + union + { + btDbvtNode* childs[2]; + void* data; + int dataAsInt; + }; }; ///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*,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 - }; - + SIMPLE_STACKSIZE = 64, + DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 + }; + // Fields btDbvtNode* m_root; btDbvtNode* m_free; int m_lkhd; int m_leaves; unsigned m_opath; + + + btAlignedObjectArray m_stkStack; + + // Methods - btDbvt(); - ~btDbvt(); + btDbvt(); + ~btDbvt(); void clear(); bool empty() const { return(0==m_root); } void optimizeBottomUp(); @@ -274,95 +271,118 @@ struct btDbvt 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 update(btDbvtNode* leaf,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& leaves); - #if DBVT_ENABLE_BENCHMARK +#if DBVT_ENABLE_BENCHMARK static void benchmark(); - #else +#else static void benchmark(){} - #endif +#endif // DBVT_IPOLICY must support ICollide policy/interface DBVT_PREFIX - static void enumNodes( const btDbvtNode* root, - DBVT_IPOLICY); + static void enumNodes( const btDbvtNode* root, + DBVT_IPOLICY); + DBVT_PREFIX + static void enumLeaves( const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX - static void enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY); + void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); + DBVT_PREFIX - static void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); + void collideTTpersistentStack( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); +#if 0 DBVT_PREFIX - static void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - DBVT_IPOLICY); + 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); + void collideTT( const btDbvtNode* root0, + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY); +#endif + DBVT_PREFIX - static void collideTV( const btDbvtNode* root, - const btDbvtVolume& volume, - DBVT_IPOLICY); + void collideTV( const btDbvtNode* root, + const btDbvtVolume& volume, + DBVT_IPOLICY); + ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc) + ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time DBVT_PREFIX - static void collideRAY( const btDbvtNode* root, - const btVector3& origin, - const btVector3& direction, - DBVT_IPOLICY); + static void rayTest( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY); + ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections + ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts DBVT_PREFIX - static void collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY); + void rayTestInternal( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayDirectionInverse, + unsigned int signs[3], + btScalar lambda_max, + const btVector3& aabbMin, + const btVector3& aabbMax, + DBVT_IPOLICY) const; + DBVT_PREFIX - static void collideOCL( const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - const btVector3& sortaxis, - int count, - DBVT_IPOLICY, - bool fullsort=true); + static void collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY); DBVT_PREFIX - static void collideTU( const btDbvtNode* root, - DBVT_IPOLICY); + 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>1; if(a[i[m]].value>=v) l=m+1; else h=m; - } - return(h); } + return(h); + } static DBVT_INLINE int allocate( btAlignedObjectArray& ifree, - btAlignedObjectArray& stock, - const sStkNPS& value) - { + btAlignedObjectArray& 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); } + { 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&) {} - }; +private: + btDbvt(const btDbvt&) {} +}; // // Inline's @@ -371,69 +391,69 @@ struct btDbvt // inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) { -btDbvtAabbMm box; -box.mi=c-e;box.mx=c+e; -return(box); + 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))); + 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); + 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;i0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); -if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); -if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); + if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); + if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); + if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); } - + // DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const { -return( (mi.x()<=a.mi.x())&& + return( (mi.x()<=a.mi.x())&& (mi.y()<=a.mi.y())&& (mi.z()<=a.mi.z())&& (mx.x()>=a.mx.x())&& @@ -444,64 +464,64 @@ return( (mi.x()<=a.mi.x())&& // DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const { -btVector3 pi,px; -switch(s) + 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; + 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; + 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; + 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; + 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; + 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; + 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; + 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; + 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); + 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)); + 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) + for(int i=0;i<3;++i) { - if(d[i]<0) + 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) + const btDbvtAabbMm& b) { #if DBVT_INT0_IMPL == DBVT_IMPL_SSE -const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), - _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); -const __int32* pu((const __int32*)&rt); -return((pu[0]|pu[1]|pu[2])==0); + const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); + const __int32* pu((const __int32*)&rt); + return((pu[0]|pu[1]|pu[2])==0); #else -return( (a.mi.x()<=b.mx.x())&& + 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())&& @@ -510,27 +530,13 @@ return( (a.mi.x()<=b.mx.x())&& #endif } -// -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) + const btVector3& b) { -return( (b.x()>=a.mi.x())&& + return( (b.x()>=a.mi.x())&& (b.y()>=a.mi.y())&& (b.z()>=a.mi.z())&& (b.x()<=a.mx.x())&& @@ -538,55 +544,40 @@ return( (b.x()>=a.mi.x())&& (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(tinx()-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(tymaxz()-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(tzmax0); -#endif -} - + + + + +////////////////////////////////////// + + // DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) + const btDbvtAabbMm& b) { -const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); -return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); + const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); + return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); } + + // DBVT_INLINE int Select( const btDbvtAabbMm& o, - const btDbvtAabbMm& a, - const btDbvtAabbMm& b) + const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { #if DBVT_SELECT_IMPL == DBVT_IMPL_SSE -static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; - // TODO: the intrinsic version is 11% slower - #if DBVT_USE_INTRINSIC_SSE + static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; + ///@todo: the intrinsic version is 11% slower +#if DBVT_USE_INTRINSIC_SSE + + union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory + { + __m128 ssereg; + float floats[4]; + int ints[4]; + }; + __m128 omi(_mm_load_ps(o.mi)); omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); __m128 ami(_mm_load_ps(a.mi)); @@ -600,74 +591,78 @@ static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7ffffff __m128 t0(_mm_movehl_ps(ami,ami)); ami=_mm_add_ps(ami,t0); ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1)); - __m128 t1(_mm_movehl_ps(bmi,bmi)); + __m128 t1(_mm_movehl_ps(bmi,bmi)); bmi=_mm_add_ps(bmi,t1); bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); - return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1); - #else - DBVT_ALIGN __int32 r[1]; + + btSSEUnion tmp; + tmp.ssereg = _mm_cmple_ss(bmi,ami); + return tmp.ints[0]&1; + +#else + ATTRIBUTE_ALIGNED16(__int32 r[1]); __asm - { + { mov eax,o - mov ecx,a - mov edx,b - movaps xmm0,[eax] + mov ecx,a + mov edx,b + movaps xmm0,[eax] movaps xmm5,mask - addps xmm0,[eax+16] + 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 - } + 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); - #endif +#endif #else -return(Proximity(o,a)b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; + if(a.mi[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) + const btDbvtAabbMm& b) { -return( (a.mi.x()!=b.mi.x())|| + 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())|| @@ -682,235 +677,383 @@ return( (a.mi.x()!=b.mi.x())|| // DBVT_PREFIX inline void btDbvt::enumNodes( const btDbvtNode* root, - DBVT_IPOLICY) + DBVT_IPOLICY) { -DBVT_CHECKTYPE -policy.Process(root); -if(root->isinternal()) + DBVT_CHECKTYPE + policy.Process(root); + if(root->isinternal()) { - enumNodes(root->childs[0],policy); - enumNodes(root->childs[1],policy); + enumNodes(root->childs[0],policy); + enumNodes(root->childs[1],policy); } } // DBVT_PREFIX inline void btDbvt::enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY) + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root->isinternal()) - { - enumLeaves(root->childs[0],policy); - enumLeaves(root->childs[1],policy); - } - else - { - policy.Process(root); - } + 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) + const btDbvtNode* root1, + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root0&&root1) - { - btAlignedObjectArray 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()) + DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + btAlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=stkStack[--depth]; + if(depth>treshold) { - 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]); + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; } - } - else if(Intersect(p.a->volume,p.b->volume)) - { - if(p.a->isinternal()) + if(p.a==p.b) { - if(p.b->isinternal()) + if(p.a->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); + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); } } - else + else if(Intersect(p.a->volume,p.b->volume)) { - if(p.b->isinternal()) + if(p.a->isinternal()) { - stack[depth++]=sStkNN(p.a,p.b->childs[0]); - stack[depth++]=sStkNN(p.a,p.b->childs[1]); + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } } else { - policy.Process(p.a,p.b); + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } } } - } - } while(depth); - } + } while(depth); + } } -// + + DBVT_PREFIX -inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - DBVT_IPOLICY) +inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root0&&root1) - { - btAlignedObjectArray 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) + DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + + m_stkStack.resize(DOUBLE_STACKSIZE); + m_stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=m_stkStack[--depth]; + if(depth>treshold) { - stack.resize(stack.size()*2); - treshold=stack.size()-4; + m_stkStack.resize(m_stkStack.size()*2); + treshold=m_stkStack.size()-4; } - if(p.a->isinternal()) + if(p.a==p.b) { - 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]); + if(p.a->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + m_stkStack[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()) + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } } else { - stack[depth++]=sStkNN(p.a->childs[0],p.b); - stack[depth++]=sStkNN(p.a->childs[1],p.b); + if(p.b->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } } } - else + } while(depth); + } +} + +#if 0 +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + btAlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=stkStack[--depth]; + if(Intersect(p.a->volume,p.b->volume,xform)) { - if(p.b->isinternal()) + if(depth>treshold) { - stack[depth++]=sStkNN(p.a,p.b->childs[0]); - stack[depth++]=sStkNN(p.a,p.b->childs[1]); + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } } else { - policy.Process(p.a,p.b); + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } } } - } - } while(depth); - } + } while(depth); + } } - // DBVT_PREFIX inline void btDbvt::collideTT( const btDbvtNode* root0, - const btTransform& xform0, - const btDbvtNode* root1, - const btTransform& xform1, - DBVT_IPOLICY) + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY) { -const btTransform xform=xform0.inverse()*xform1; -collideTT(root0,root1,xform,policy); + const btTransform xform=xform0.inverse()*xform1; + collideTT(root0,root1,xform,policy); } +#endif // DBVT_PREFIX inline void btDbvt::collideTV( const btDbvtNode* root, - const btDbvtVolume& vol, - DBVT_IPOLICY) + const btDbvtVolume& vol, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + btAlignedObjectArray stack; + stack.resize(0); + 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::rayTestInternal( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayDirectionInverse, + unsigned int signs[3], + btScalar lambda_max, + const btVector3& aabbMin, + const btVector3& aabbMax, + DBVT_IPOLICY) const { -DBVT_CHECKTYPE -if(root) + DBVT_CHECKTYPE + if(root) { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - btAlignedObjectArray 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)) + btVector3 resultNormal; + + int depth=1; + int treshold=DOUBLE_STACKSIZE-2; + btAlignedObjectArray stack; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=root; + btVector3 bounds[2]; + do + { + const btDbvtNode* node=stack[--depth]; + bounds[0] = node->volume.Mins()+aabbMin; + bounds[1] = node->volume.Maxs()+aabbMax; + btScalar tmin=1.f,lambda_min=0.f; + unsigned int result1=false; + result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + if(result1) { - if(n->isinternal()) + if(node->isinternal()) { - stack.push_back(n->childs[0]); - stack.push_back(n->childs[1]); + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; } else { - policy.Process(n); + policy.Process(node); } } - } while(stack.size()>0); + } while(depth); } } // DBVT_PREFIX -inline void btDbvt::collideRAY( const btDbvtNode* root, - const btVector3& origin, - const btVector3& direction, - DBVT_IPOLICY) +inline void btDbvt::rayTest( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + 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 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 + DBVT_CHECKTYPE + if(root) + { + btVector3 rayDir = (rayTo-rayFrom); + rayDir.normalize (); + + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + + btScalar lambda_max = rayDir.dot(rayTo-rayFrom); + + btVector3 resultNormal; + + btAlignedObjectArray stack; + + int depth=1; + int treshold=DOUBLE_STACKSIZE-2; + + stack.resize(DOUBLE_STACKSIZE); + stack[0]=root; + btVector3 bounds[2]; + do { + const btDbvtNode* node=stack[--depth]; + + bounds[0] = node->volume.Mins(); + bounds[1] = node->volume.Maxs(); + + btScalar tmin=1.f,lambda_min=0.f; + unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + +#ifdef COMPARE_BTRAY_AABB2 + btScalar param=1.f; + bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); + btAssert(result1 == result2); +#endif //TEST_BTRAY_AABB2 + + if(result1) { - policy.Process(node); + if(node->isinternal()) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; + } + else + { + policy.Process(node); + } } - } - } while(stack.size()); - } + } while(depth); + + } } // @@ -921,174 +1064,174 @@ inline void btDbvt::collideKDOP(const btDbvtNode* root, int count, DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root) - { - const int inside=(1< stack; - int signs[sizeof(unsigned)*8]; - btAssert(count=0)?1:0)+ + const int inside=(1< stack; + int signs[sizeof(unsigned)*8]; + btAssert(count=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)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); - switch(side) + if(0==(se.mask&j)) { - case -1: out=true;break; - case +1: se.mask|=j;break; + 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(!out) { - if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); + 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()); - } + } 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) + 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< stock; - btAlignedObjectArray ifree; - btAlignedObjectArray stack; - int signs[sizeof(unsigned)*8]; - btAssert(count=0)?1:0)+ + const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ + (sortaxis[1]>=0?2:0)+ + (sortaxis[2]>=0?4:0); + const int inside=(1< stock; + btAlignedObjectArray ifree; + btAlignedObjectArray stack; + int signs[sizeof(unsigned)*8]; + btAssert(count=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)&&(ivolume.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) { - if(0==(se.mask&j)) + bool out=false; + for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); - switch(side) + if(0==(se.mask&j)) { - case -1: out=true;break; - case +1: se.mask|=j;break; + 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(out) continue; - } - if(policy.Descent(se.node)) - { - if(se.node->isinternal()) + if(policy.Descent(se.node)) { - 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].value0)) + if(se.node->isinternal()) { - /* 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]); + 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].value0)) + { + /* 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 { - stack.push_back(allocate(ifree,stock,nes[q])); - stack.push_back(allocate(ifree,stock,nes[1-q])); + policy.Process(se.node,se.value); } } - else - { - policy.Process(se.node,se.value); - } - } - } while(stack.size()); - } + } while(stack.size()); + } } // DBVT_PREFIX inline void btDbvt::collideTU( const btDbvtNode* root, - DBVT_IPOLICY) + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root) - { - btAlignedObjectArray 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); - } + DBVT_CHECKTYPE + if(root) + { + btAlignedObjectArray 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); + } } // diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index e00fc6aa5e3..f231717af17 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -26,19 +26,19 @@ subject to the following restrictions: #if DBVT_BP_PROFILE struct ProfileScope - { +{ __forceinline ProfileScope(btClock& clock,unsigned long& value) : - m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) - { - } + m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + { + } __forceinline ~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_) @@ -52,35 +52,35 @@ struct ProfileScope template static inline void listappend(T* item,T*& list) { -item->links[0]=0; -item->links[1]=list; -if(list) list->links[0]=item; -list=item; + item->links[0]=0; + item->links[1]=list; + if(list) list->links[0]=item; + list=item; } // template 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]; + 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 static inline int listcount(T* root) { -int n=0; -while(root) { ++n;root=root->links[1]; } -return(n); + int n=0; + while(root) { ++n;root=root->links[1]; } + return(n); } // template static inline void clear(T& value) { -static const struct ZeroDummy : T {} zerodummy; -value=zerodummy; + static const struct ZeroDummy : T {} zerodummy; + value=zerodummy; } // @@ -90,25 +90,26 @@ value=zerodummy; /* Tree collider */ struct btDbvtTreeCollider : btDbvt::ICollide { -btDbvtBroadphase* pbp; -btDbvtProxy* proxy; - btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} -void Process(const btDbvtNode* na,const btDbvtNode* nb) + btDbvtBroadphase* pbp; + btDbvtProxy* proxy; + btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} + void Process(const btDbvtNode* na,const btDbvtNode* nb) { - if(na!=nb) + if(na!=nb) { - btDbvtProxy* pa=(btDbvtProxy*)na->data; - btDbvtProxy* pb=(btDbvtProxy*)nb->data; - #if DBVT_BP_SORTPAIRS - if(pa>pb) btSwap(pa,pb); - #endif - pbp->m_paircache->addOverlappingPair(pa,pb); - ++pbp->m_newpairs; + btDbvtProxy* pa=(btDbvtProxy*)na->data; + btDbvtProxy* pb=(btDbvtProxy*)nb->data; +#if DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + btSwap(pa,pb); +#endif + pbp->m_paircache->addOverlappingPair(pa,pb); + ++pbp->m_newpairs; } } -void Process(const btDbvtNode* n) + void Process(const btDbvtNode* n) { - Process(n,proxy->leaf); + Process(n,proxy->leaf); } }; @@ -119,147 +120,197 @@ void Process(const btDbvtNode* n) // btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) { -m_deferedcollide = false; -m_needcleanup = true; -m_releasepaircache = (paircache!=0)?false:true; -m_prediction = 1/(btScalar)2; -m_stageCurrent = 0; -m_fixedleft = 0; -m_fupdates = 1; -m_dupdates = 0; -m_cupdates = 10; -m_newpairs = 1; -m_updates_call = 0; -m_updates_done = 0; -m_updates_ratio = 0; -m_paircache = paircache? - paircache : - new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); -m_gid = 0; -m_pid = 0; -m_cid = 0; -for(int i=0;i<=STAGECOUNT;++i) + m_deferedcollide = false; + m_needcleanup = true; + m_releasepaircache = (paircache!=0)?false:true; + m_prediction = 1/(btScalar)2; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); + m_gid = 0; + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) { - m_stageRoots[i]=0; + m_stageRoots[i]=0; } #if DBVT_BP_PROFILE -clear(m_profiling); + clear(m_profiling); #endif } // btDbvtBroadphase::~btDbvtBroadphase() { -if(m_releasepaircache) -{ - m_paircache->~btOverlappingPairCache(); - btAlignedFree(m_paircache); -} + 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*/) + 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->stage = m_stageCurrent; -proxy->m_uniqueId = ++m_gid; -proxy->leaf = m_sets[0].insert(proxy->aabb,proxy); -listappend(proxy,m_stageRoots[m_stageCurrent]); -if(!m_deferedcollide) + btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, + collisionFilterGroup, + collisionFilterMask); + + btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); + + //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); + proxy->stage = m_stageCurrent; + proxy->m_uniqueId = ++m_gid; + proxy->leaf = m_sets[0].insert(aabb,proxy); + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(!m_deferedcollide) { - btDbvtTreeCollider collider(this); - collider.proxy=proxy; - btDbvt::collideTV(m_sets[0].m_root,proxy->aabb,collider); - btDbvt::collideTV(m_sets[1].m_root,proxy->aabb,collider); + btDbvtTreeCollider collider(this); + collider.proxy=proxy; + m_sets[0].collideTV(m_sets[0].m_root,aabb,collider); + m_sets[1].collideTV(m_sets[1].m_root,aabb,collider); } -return(proxy); + return(proxy); } // void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, - btDispatcher* dispatcher) + btDispatcher* dispatcher) { -btDbvtProxy* proxy=(btDbvtProxy*)absproxy; -if(proxy->stage==STAGECOUNT) - m_sets[1].remove(proxy->leaf); + 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); -m_needcleanup=true; + m_sets[0].remove(proxy->leaf); + listremove(proxy,m_stageRoots[proxy->stage]); + m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); + btAlignedFree(proxy); + m_needcleanup=true; +} + +void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + aabbMin = proxy->m_aabbMin; + aabbMax = proxy->m_aabbMax; +} + +struct BroadphaseRayTester : btDbvt::ICollide +{ + btBroadphaseRayCallback& m_rayCallback; + BroadphaseRayTester(btBroadphaseRayCallback& orgCallback) + :m_rayCallback(orgCallback) + { + } + void Process(const btDbvtNode* leaf) + { + btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; + m_rayCallback.process(proxy); + } +}; + +void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) +{ + BroadphaseRayTester callback(rayCallback); + + m_sets[0].rayTestInternal( m_sets[0].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + callback); + + m_sets[1].rayTestInternal( m_sets[1].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + callback); + } // void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, - const btVector3& aabbMin, - const btVector3& aabbMax, - btDispatcher* /*dispatcher*/) + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* /*dispatcher*/) { -btDbvtProxy* proxy=(btDbvtProxy*)absproxy; -ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); #if DBVT_BP_PREVENTFALSEUPDATE -if(NotEqual(aabb,proxy->leaf->volume)) + if(NotEqual(aabb,proxy->leaf->volume)) #endif { - bool docollide=false; - if(proxy->stage==STAGECOUNT) + bool docollide=false; + if(proxy->stage==STAGECOUNT) {/* fixed -> dynamic set */ - m_sets[1].remove(proxy->leaf); - proxy->leaf=m_sets[0].insert(aabb,proxy); - docollide=true; + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); + docollide=true; } else {/* dynamic set */ - ++m_updates_call; - if(Intersect(proxy->leaf->volume,aabb)) + ++m_updates_call; + if(Intersect(proxy->leaf->volume,aabb)) {/* Moving */ - const btVector3 delta=aabbMin-proxy->aabb.Mins(); - btVector3 velocity(aabb.Extents()*m_prediction); - if(delta[0]<0) velocity[0]=-velocity[0]; - if(delta[1]<0) velocity[1]=-velocity[1]; - if(delta[2]<0) velocity[2]=-velocity[2]; - if ( - #ifdef DBVT_BP_MARGIN - m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) - #else - m_sets[0].update(proxy->leaf,aabb,velocity) - #endif - ) + + const btVector3 delta=aabbMin-proxy->m_aabbMin; + btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); + if(delta[0]<0) velocity[0]=-velocity[0]; + if(delta[1]<0) velocity[1]=-velocity[1]; + if(delta[2]<0) velocity[2]=-velocity[2]; + if ( +#ifdef DBVT_BP_MARGIN + m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) +#else + m_sets[0].update(proxy->leaf,aabb,velocity) +#endif + ) { - ++m_updates_done; - docollide=true; + ++m_updates_done; + docollide=true; } } else {/* Teleporting */ - m_sets[0].update(proxy->leaf,aabb); - ++m_updates_done; - docollide=true; + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; } } - listremove(proxy,m_stageRoots[proxy->stage]); - proxy->aabb = aabb; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(docollide) + listremove(proxy,m_stageRoots[proxy->stage]); + proxy->m_aabbMin = aabbMin; + proxy->m_aabbMax = aabbMax; + proxy->stage = m_stageCurrent; + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) { - m_needcleanup=true; - if(!m_deferedcollide) + m_needcleanup=true; + if(!m_deferedcollide) { - btDbvtTreeCollider collider(this); - btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider); - btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider); + btDbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); } } } @@ -268,132 +319,226 @@ if(NotEqual(aabb,proxy->leaf->volume)) // void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { -collide(dispatcher); + collide(dispatcher); #if DBVT_BP_PROFILE -if(0==(m_pid%DBVT_BP_PROFILING_RATE)) + 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(); + 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 + + performDeferredRemoval(dispatcher); + +} + +void btDbvtBroadphase::performDeferredRemoval(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()); + + int invalidPair = 0; + + + int i; + + btBroadphasePair previousPair; + previousPair.m_pProxy0 = 0; + previousPair.m_pProxy1 = 0; + previousPair.m_algorithm = 0; + + + for (i=0;ileaf->volume,pb->leaf->volume); + + if (hasOverlap) + { + needsRemoval = false; + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + m_paircache->cleanOverlappingPair(pair,dispatcher); + + pair.m_pProxy0 = 0; + pair.m_pProxy1 = 0; + invalidPair++; + } + + } + + //perform a sort, to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + overlappingPairArray.resize(overlappingPairArray.size() - invalidPair); + } } // void btDbvtBroadphase::collide(btDispatcher* dispatcher) { -SPC(m_profiling.m_total); -/* optimize */ -m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); -if(m_fixedleft) + /*printf("---------------------------------------------------------\n"); + printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves); + printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves); + printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs()); { - const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; - m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); - m_fixedleft=btMax(0,m_fixedleft-count); + int i; + for (i=0;igetNumOverlappingPairs();i++) + { + printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(), + getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid()); + } + printf("\n"); } -/* dynamic -> fixed set */ -m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; -btDbvtProxy* current=m_stageRoots[m_stageCurrent]; -if(current) +*/ + + + + SPC(m_profiling.m_total); + /* optimize */ + m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); + if(m_fixedleft) + { + const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; + m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); + m_fixedleft=btMax(0,m_fixedleft-count); + } + /* 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]); - #if DBVT_BP_ACCURATESLEEPING - m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); - collider.proxy=current; - btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); - btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); - #endif - m_sets[0].remove(current->leaf); - current->leaf = m_sets[1].insert(current->aabb,current); - current->stage = STAGECOUNT; - current = next; + btDbvtTreeCollider collider(this); + do { + btDbvtProxy* next=current->links[1]; + listremove(current,m_stageRoots[current->stage]); + listappend(current,m_stageRoots[STAGECOUNT]); +#if DBVT_BP_ACCURATESLEEPING + m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); + collider.proxy=current; + btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); + btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); +#endif + m_sets[0].remove(current->leaf); + ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); + current->leaf = m_sets[1].insert(curAabb,current); + current->stage = STAGECOUNT; + current = next; } while(current); - m_fixedleft=m_sets[1].m_leaves; - m_needcleanup=true; + m_fixedleft=m_sets[1].m_leaves; + m_needcleanup=true; } -/* collide dynamics */ + /* collide dynamics */ { - btDbvtTreeCollider collider(this); - if(m_deferedcollide) + btDbvtTreeCollider collider(this); + if(m_deferedcollide) { - SPC(m_profiling.m_fdcollide); - btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider); + SPC(m_profiling.m_fdcollide); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); } - if(m_deferedcollide) + if(m_deferedcollide) { - SPC(m_profiling.m_ddcollide); - btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider); + SPC(m_profiling.m_ddcollide); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); } } -/* clean up */ -if(m_needcleanup) + /* clean up */ + if(m_needcleanup) { - SPC(m_profiling.m_cleanup); - btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); - if(pairs.size()>0) + SPC(m_profiling.m_cleanup); + btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + if(pairs.size()>0) { - const int ci=pairs.size(); - int ni=btMin(ci,btMax(m_newpairs,(ci*m_cupdates)/100)); - for(int i=0;i(m_newpairs,(pairs.size()*m_cupdates)/100)); + for(int i=0;ileaf->volume,pb->leaf->volume)) + btBroadphasePair& p=pairs[(m_cid+i)%pairs.size()]; + btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0; + btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1; + if(!Intersect(pa->leaf->volume,pb->leaf->volume)) { - #if DBVT_BP_SORTPAIRS - if(pa>pb) btSwap(pa,pb); - #endif - m_paircache->removeOverlappingPair(pa,pb,dispatcher); - --ni;--i; +#if DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + btSwap(pa,pb); +#endif + m_paircache->removeOverlappingPair(pa,pb,dispatcher); + --ni;--i; } } - if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; + if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; } } -++m_pid; -m_newpairs=1; -m_needcleanup=false; -if(m_updates_call>0) + ++m_pid; + m_newpairs=1; + m_needcleanup=false; + if(m_updates_call>0) { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; } else { m_updates_ratio=0; } -m_updates_done/=2; -m_updates_call/=2; + m_updates_done/=2; + m_updates_call/=2; } // void btDbvtBroadphase::optimize() { -m_sets[0].optimizeTopDown(); -m_sets[1].optimizeTopDown(); + m_sets[0].optimizeTopDown(); + m_sets[1].optimizeTopDown(); } // btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() { -return(m_paircache); + return(m_paircache); } // const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const { -return(m_paircache); + return(m_paircache); } // @@ -402,16 +547,49 @@ void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aab 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(); + 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::resetPool(btDispatcher* dispatcher) +{ + + int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves; + if (!totalObjects) + { + //reset internal dynamic tree data structures + m_sets[0].clear(); + m_sets[1].clear(); + + m_deferedcollide = false; + m_needcleanup = true; + m_prediction = 1/(btScalar)2; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + + m_gid = 0; + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } + } } // @@ -422,9 +600,9 @@ void btDbvtBroadphase::printStats() #if DBVT_BP_ENABLE_BENCHMARK struct btBroadphaseBenchmark - { +{ struct Experiment - { + { const char* name; int object_count; int update_count; @@ -432,109 +610,109 @@ struct btBroadphaseBenchmark int iterations; btScalar speed; btScalar amplitude; - }; + }; struct Object - { + { btVector3 center; btVector3 extents; btBroadphaseProxy* proxy; btScalar time; void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) - { + { time += speed; center[0] = btCos(time*(btScalar)2.17)*amplitude+ - btSin(time)*amplitude/2; + btSin(time)*amplitude/2; center[1] = btCos(time*(btScalar)1.38)*amplitude+ - btSin(time)*amplitude; + btSin(time)*amplitude; center[2] = btSin(time*(btScalar)0.777)*amplitude; pbi->setAabb(proxy,center-extents,center+extents,0); - } - }; + } + }; static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } static void OutputTime(const char* name,btClock& c,unsigned count=0) - { + { const unsigned long us=c.getTimeMicroseconds(); const unsigned long ms=(us+500)/1000; const btScalar sec=us/(btScalar)(1000*1000); if(count>0) printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); - else + else printf("%s : %u us (%u ms)\r\n",name,us,ms); - } - }; + } +}; void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) { -static const btBroadphaseBenchmark::Experiment experiments[]= + static const btBroadphaseBenchmark::Experiment experiments[]= { - {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, - /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, - {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ + {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, + /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, + {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ }; -static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); -btAlignedObjectArray objects; -btClock wallclock; -/* Begin */ -for(int iexp=0;iexp objects; + btClock wallclock; + /* Begin */ + for(int iexp=0;iexpcenter[0]=btBroadphaseBenchmark::UnitRand()*50; - po->center[1]=btBroadphaseBenchmark::UnitRand()*50; - po->center[2]=btBroadphaseBenchmark::UnitRand()*50; - po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; - po->time=btBroadphaseBenchmark::UnitRand()*2000; - po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); - objects.push_back(po); + btBroadphaseBenchmark::Object* po=new btBroadphaseBenchmark::Object(); + po->center[0]=btBroadphaseBenchmark::UnitRand()*50; + po->center[1]=btBroadphaseBenchmark::UnitRand()*50; + po->center[2]=btBroadphaseBenchmark::UnitRand()*50; + po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; + po->time=btBroadphaseBenchmark::UnitRand()*2000; + po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); + objects.push_back(po); } - btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); - /* First update */ - wallclock.reset(); - for(int i=0;iupdate(speed,amplitude,pbi); + objects[i]->update(speed,amplitude,pbi); } - btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); - /* Updates */ - wallclock.reset(); - for(int i=0;iupdate(speed,amplitude,pbi); + objects[j]->update(speed,amplitude,pbi); } - pbi->calculateOverlappingPairs(0); + pbi->calculateOverlappingPairs(0); } - btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); - /* Clean up */ - wallclock.reset(); - for(int i=0;idestroyProxy(objects[i]->proxy,0); - delete objects[i]; + pbi->destroyProxy(objects[i]->proxy,0); + delete objects[i]; } - objects.resize(0); - btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); + objects.resize(0); + btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); } } @@ -546,3 +724,4 @@ void btDbvtBroadphase::benchmark(btBroadphaseInterface*) #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 index 1f16043a7a8..fe70bc39c43 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -24,15 +24,15 @@ subject to the following restrictions: // #define DBVT_BP_PROFILE 0 -#define DBVT_BP_SORTPAIRS 1 +//#define DBVT_BP_SORTPAIRS 1 #define DBVT_BP_PREVENTFALSEUPDATE 0 #define DBVT_BP_ACCURATESLEEPING 0 #define DBVT_BP_ENABLE_BENCHMARK 0 #define DBVT_BP_MARGIN (btScalar)0.05 #if DBVT_BP_PROFILE - #define DBVT_BP_PROFILING_RATE 256 - #include "LinearMath/btQuickprof.h" +#define DBVT_BP_PROFILING_RATE 256 +#include "LinearMath/btQuickprof.h" #endif // @@ -40,16 +40,16 @@ subject to the following restrictions: // 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) + /* Fields */ + //btDbvtAabbMm aabb; + btDbvtNode* leaf; + btDbvtProxy* links[2]; + int stage; + /* ctor */ + btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : + btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) { - links[0]=links[1]=0; + links[0]=links[1]=0; } }; @@ -60,57 +60,67 @@ typedef btAlignedObjectArray btDbvtProxyArray; ///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 { + /* 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_prediction; // Velocity prediction -int m_stageCurrent; // Current stage -int m_fupdates; // % of fixed updates per frame -int m_dupdates; // % of dynamic updates per frame -int m_cupdates; // % of cleanup updates per frame -int m_newpairs; // Number of pairs created -int m_fixedleft; // Fixed optimization left -unsigned m_updates_call; // Number of updates call -unsigned m_updates_done; // Number of updates done -btScalar m_updates_ratio; // m_updates_done/m_updates_call -int m_pid; // Parse id -int m_cid; // Cleanup index -int m_gid; // Gen id -bool m_releasepaircache; // Release pair cache on delete -bool m_deferedcollide; // Defere dynamic/static collision to collide call -bool m_needcleanup; // Need to run cleanup? + }; + /* Fields */ + btDbvt m_sets[2]; // Dbvt sets + btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list + btOverlappingPairCache* m_paircache; // Pair cache + btScalar m_prediction; // Velocity prediction + int m_stageCurrent; // Current stage + int m_fupdates; // % of fixed updates per frame + int m_dupdates; // % of dynamic updates per frame + int m_cupdates; // % of cleanup updates per frame + int m_newpairs; // Number of pairs created + int m_fixedleft; // Fixed optimization left + unsigned m_updates_call; // Number of updates call + unsigned m_updates_done; // Number of updates done + btScalar m_updates_ratio; // m_updates_done/m_updates_call + int m_pid; // Parse id + int m_cid; // Cleanup index + int m_gid; // Gen id + bool m_releasepaircache; // Release pair cache on delete + bool m_deferedcollide; // Defere dynamic/static collision to collide call + bool m_needcleanup; // Need to run cleanup? #if DBVT_BP_PROFILE -btClock m_clock; -struct { + 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; + } 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(); -static void benchmark(btBroadphaseInterface*); + /* 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); + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); + + virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + void calculateOverlappingPairs(btDispatcher* dispatcher); + btOverlappingPairCache* getOverlappingPairCache(); + const btOverlappingPairCache* getOverlappingPairCache() const; + void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; + void printStats(); + static void benchmark(btBroadphaseInterface*); + + + void performDeferredRemoval(btDispatcher* dispatcher); + + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(btDispatcher* dispatcher); + }; #endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 6db71a0170e..ee57aa96151 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -46,22 +46,25 @@ struct btDispatcherInfo m_enableSPU(true), m_useEpa(true), m_allowedCcdPenetration(btScalar(0.04)), + m_useConvexConservativeDistanceUtil(true), + m_convexConservativeDistanceThreshold(0.0f), m_stackAllocator(0) { } btScalar m_timeStep; - int m_stepCount; - int m_dispatchFunc; + int m_stepCount; + int m_dispatchFunc; mutable btScalar m_timeOfImpact; - bool m_useContinuous; + bool m_useContinuous; class btIDebugDraw* m_debugDraw; - bool m_enableSatConvex; - bool m_enableSPU; - bool m_useEpa; + bool m_enableSatConvex; + bool m_enableSPU; + bool m_useEpa; btScalar m_allowedCcdPenetration; + bool m_useConvexConservativeDistanceUtil; + btScalar m_convexConservativeDistanceThreshold; btStackAlloc* m_stackAllocator; - }; ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs. diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp index 3f866ab7c5f..6712f528e97 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp @@ -149,6 +149,22 @@ amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ(); +void btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + btMultiSapProxy* multiProxy = static_cast(proxy); + aabbMin = multiProxy->m_aabbMin; + aabbMax = multiProxy->m_aabbMax; +} + +void btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) +{ + for (int i=0;i void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) @@ -208,7 +224,9 @@ void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab - m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); + if (m_optimizedAabbTree) + m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); + int i; for ( i=0;im_bridgeProxies.size();i++) @@ -464,3 +482,8 @@ void btMultiSapBroadphase::printStats() */ } + +void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher) +{ + // not yet +} diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h index a0c002de856..91c504eee22 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h @@ -26,6 +26,7 @@ class btSimpleBroadphase; typedef btAlignedObjectArray btSapBroadphaseArray; +///The btMultiSapBroadphase is a research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead. ///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. @@ -72,7 +73,7 @@ public: 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), + :btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask), m_aabbMin(aabbMin), m_aabbMax(aabbMax), m_shapeType(shapeType) @@ -108,6 +109,9 @@ public: 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); + virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); void addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase); @@ -139,6 +143,9 @@ public: void quicksort (btBroadphasePairArray& a, int lo, int hi); + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(btDispatcher* dispatcher); + }; #endif //BT_MULTI_SAP_BROADPHASE diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index ff65cdde79f..b209bcb9a20 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -19,6 +19,7 @@ subject to the following restrictions: #include "btDispatcher.h" #include "btCollisionAlgorithm.h" +#include "LinearMath/btAabbUtil2.h" #include @@ -33,7 +34,8 @@ int gFindPairs =0; btHashedOverlappingPairCache::btHashedOverlappingPairCache(): m_overlapFilterCallback(0), - m_blockedForChanges(false) + m_blockedForChanges(false), + m_ghostPairCallback(0) { int initialAllocatedSize= 2; m_overlappingPairArray.reserve(initialAllocatedSize); @@ -45,7 +47,6 @@ btHashedOverlappingPairCache::btHashedOverlappingPairCache(): btHashedOverlappingPairCache::~btHashedOverlappingPairCache() { - //todo/test: show we erase/delete data, or is it automatic } @@ -135,7 +136,8 @@ void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroad btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { gFindPairs++; - if(proxy0>proxy1) btSwap(proxy0,proxy1); + if(proxy0->m_uniqueId>proxy1->m_uniqueId) + btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); @@ -211,7 +213,8 @@ void btHashedOverlappingPairCache::growTables() btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - if(proxy0>proxy1) btSwap(proxy0,proxy1); + if(proxy0->m_uniqueId>proxy1->m_uniqueId) + btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); @@ -238,6 +241,11 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx int count = m_overlappingPairArray.size(); int oldCapacity = m_overlappingPairArray.capacity(); void* mem = &m_overlappingPairArray.expand(); + + //this is where we add an actual pair, so also call the 'ghost' + if (m_ghostPairCallback) + m_ghostPairCallback->addOverlappingPair(proxy0,proxy1); + int newCapacity = m_overlappingPairArray.capacity(); if (oldCapacity < newCapacity) @@ -251,7 +259,7 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx // pair->m_pProxy0 = proxy0; // pair->m_pProxy1 = proxy1; pair->m_algorithm = 0; - pair->m_userInfo = 0; + pair->m_internalTmpValue = 0; m_next[count] = m_hashTable[hash]; @@ -265,7 +273,8 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) { gRemovePairs++; - if(proxy0>proxy1) btSwap(proxy0,proxy1); + if(proxy0->m_uniqueId>proxy1->m_uniqueId) + btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); @@ -282,7 +291,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro cleanOverlappingPair(*pair,dispatcher); - void* userData = pair->m_userInfo; + void* userData = pair->m_internalInfo1; btAssert(pair->m_pProxy0->getUid() == proxyId1); btAssert(pair->m_pProxy1->getUid() == proxyId2); @@ -317,6 +326,9 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro int lastPairIndex = m_overlappingPairArray.size() - 1; + if (m_ghostPairCallback) + m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); + // If the removed pair is the last pair, we are done. if (lastPairIndex == pairIndex) { @@ -384,6 +396,35 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* } } +void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) +{ + ///need to keep hashmap in sync with pair address, so rebuild all + btBroadphasePairArray tmpPairs; + int i; + for (i=0;iremoveOverlappingPair(proxy0, proxy1,dispatcher); m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); m_overlappingPairArray.pop_back(); @@ -419,15 +462,19 @@ void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) { //don't add overlap with own - assert(proxy0 != proxy1); + btAssert(proxy0 != proxy1); if (!needsBroadphaseCollision(proxy0,proxy1)) return 0; void* mem = &m_overlappingPairArray.expand(); btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); + gOverlappingPairs++; gAddedPairs++; + + if (m_ghostPairCallback) + m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); return pair; } @@ -446,7 +493,7 @@ btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP if (findIndex < m_overlappingPairArray.size()) { - //assert(it != m_overlappingPairSet.end()); + //btAssert(it != m_overlappingPairSet.end()); btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; return pair; } @@ -476,8 +523,9 @@ void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* if (callback->processOverlap(*pair)) { cleanOverlappingPair(*pair,dispatcher); - - m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1); + pair->m_pProxy0 = 0; + pair->m_pProxy1 = 0; + m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); m_overlappingPairArray.pop_back(); gOverlappingPairs--; } else @@ -493,7 +541,8 @@ void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* btSortedOverlappingPairCache::btSortedOverlappingPairCache(): m_blockedForChanges(false), m_hasDeferredRemoval(true), - m_overlapFilterCallback(0) + m_overlapFilterCallback(0), + m_ghostPairCallback(0) { int initialAllocatedSize= 2; m_overlappingPairArray.reserve(initialAllocatedSize); @@ -501,7 +550,6 @@ btSortedOverlappingPairCache::btSortedOverlappingPairCache(): btSortedOverlappingPairCache::~btSortedOverlappingPairCache() { - //todo/test: show we erase/delete data, or is it automatic } void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) @@ -577,3 +625,9 @@ void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroad processAllOverlappingPairs(&removeCallback,dispatcher); } + +void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) +{ + //should already be sorted +} + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 66679bd218a..eda45c47b5b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -21,7 +21,6 @@ subject to the following restrictions: #include "btBroadphaseProxy.h" #include "btOverlappingPairCallback.h" -#include "LinearMath/btPoint3.h" #include "LinearMath/btAlignedObjectArray.h" class btDispatcher; @@ -83,6 +82,11 @@ public: virtual bool hasDeferredRemoval() = 0; + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0; + + virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0; + + }; /// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com @@ -253,10 +257,19 @@ private: return false; } -public: + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + + virtual void sortOverlappingPairs(btDispatcher* dispatcher); + + +protected: btAlignedObjectArray m_hashTable; btAlignedObjectArray m_next; + btOverlappingPairCallback* m_ghostPairCallback; }; @@ -280,6 +293,8 @@ class btSortedOverlappingPairCache : public btOverlappingPairCache //if set, use the callback instead of the built in filter in needBroadphaseCollision btOverlapFilterCallback* m_overlapFilterCallback; + btOverlappingPairCallback* m_ghostPairCallback; + public: btSortedOverlappingPairCache(); @@ -355,12 +370,19 @@ class btSortedOverlappingPairCache : public btOverlappingPairCache return m_hasDeferredRemoval; } + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + + virtual void sortOverlappingPairs(btDispatcher* dispatcher); + }; -///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and testing. +///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing. class btNullPairCache : public btOverlappingPairCache { @@ -414,6 +436,11 @@ public: return true; } + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) + { + + } + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/) { return 0; @@ -427,6 +454,10 @@ public: virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) { } + + virtual void sortOverlappingPairs(btDispatcher* dispatcher) + { + } }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index a30bd1fd9e1..8bef8f0d43e 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -18,14 +18,18 @@ subject to the following restrictions: #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btIDebugDraw.h" +#define RAYAABB2 -btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false), +btQuantizedBvh::btQuantizedBvh() : + m_bulletVersion(BT_BULLET_VERSION), + m_useQuantization(false), //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) m_traversalMode(TRAVERSAL_STACKLESS) //m_traversalMode(TRAVERSAL_RECURSIVE) ,m_subtreeHeaderCount(0) //PCK: add this line -{ - +{ + m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY); + m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); } @@ -119,7 +123,7 @@ void btQuantizedBvh::buildTree (int startIndex,int endIndex) int numIndices =endIndex-startIndex; int curIndex = m_curNodeIndex; - assert(numIndices>0); + btAssert(numIndices>0); if (numIndices==1) { @@ -140,8 +144,11 @@ void btQuantizedBvh::buildTree (int startIndex,int endIndex) int internalNodeIndex = m_curNodeIndex; - setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin); - setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax); + //set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value. + //the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values + setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization + setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization + for (i=startIndex;im_aabbMinOrg,rootNode->m_aabbMaxOrg); @@ -434,6 +445,96 @@ void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantize +void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const +{ + btAssert(!m_useQuantization); + + const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0]; + int escapeIndex, curIndex = 0; + int walkIterations = 0; + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap=0; + unsigned rayBoxOverlap=0; + btScalar lambda_max = 1.0; + + /* Quick pruning by quantized box */ + btVector3 rayAabbMin = raySource; + btVector3 rayAabbMax = raySource; + rayAabbMin.setMin(rayTarget); + rayAabbMax.setMax(rayTarget); + + /* Add box cast extents to bounding box */ + rayAabbMin += aabbMin; + rayAabbMax += aabbMax; + +#ifdef RAYAABB2 + btVector3 rayDir = (rayTarget-raySource); + rayDir.normalize (); + lambda_max = rayDir.dot(rayTarget-raySource); + ///what about division by zero? --> just set rayDirection[i] to 1.0 + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; +#endif + + btVector3 bounds[2]; + + while (curIndex < m_curNodeIndex) + { + btScalar param = 1.0; + //catch bugs in tree data + btAssert (walkIterations < m_curNodeIndex); + + walkIterations++; + + bounds[0] = rootNode->m_aabbMinOrg; + bounds[1] = rootNode->m_aabbMaxOrg; + /* Add box cast extents */ + bounds[0] += aabbMin; + bounds[1] += aabbMax; + + aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg); + //perhaps profile if it is worth doing the aabbOverlap test first + +#ifdef RAYAABB2 + ///careful with this check: need to check division by zero (above) and fix the unQuantize method + ///thanks Joerg/hiker for the reproduction case! + ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 + rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false; + +#else + btVector3 normal; + rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal); +#endif + + isLeafNode = rootNode->m_escapeIndex == -1; + + //PCK: unsigned instead of bool + if (isLeafNode && (rayBoxOverlap != 0)) + { + nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); + } + + //PCK: unsigned instead of bool + if ((rayBoxOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->m_escapeIndex; + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const @@ -454,9 +555,8 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* unsigned rayBoxOverlap = 0; btScalar lambda_max = 1.0; -#define RAYAABB2 + #ifdef RAYAABB2 - btVector3 rayFrom = raySource; btVector3 rayDirection = (rayTarget-raySource); rayDirection.normalize (); lambda_max = rayDirection.dot(rayTarget-raySource); @@ -502,7 +602,7 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* #endif//VISUALLY_ANALYZE_BVH //catch bugs in tree data - assert (walkIterations < subTreeSize); + btAssert (walkIterations < subTreeSize); walkIterations++; //PCK: unsigned instead of bool @@ -533,7 +633,9 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* ///thanks Joerg/hiker for the reproduction case! ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 + //BT_PROFILE("btRayAabb2"); rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); + #else rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); #endif @@ -597,7 +699,7 @@ void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb #endif//VISUALLY_ANALYZE_BVH //catch bugs in tree data - assert (walkIterations < subTreeSize); + btAssert (walkIterations < subTreeSize); walkIterations++; //PCK: unsigned instead of bool @@ -652,30 +754,25 @@ void btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallba void btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const { - bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; - if (fast_path) - { - walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex); - } else { - /* Otherwise fallback to AABB overlap test */ - btVector3 aabbMin = raySource; - btVector3 aabbMax = raySource; - aabbMin.setMin(rayTarget); - aabbMax.setMax(rayTarget); - reportAabbOverlappingNodex(nodeCallback,aabbMin,aabbMax); - } + reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0)); } void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const { - bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; - if (fast_path) + //always use stackless + + if (m_useQuantization) { walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); - } else { - /* Slow path: - Construct the bounding box for the entire box cast and send that down the tree */ + } + else + { + walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); + } + /* + { + //recursive traversal btVector3 qaabbMin = raySource; btVector3 qaabbMax = raySource; qaabbMin.setMin(rayTarget); @@ -684,6 +781,8 @@ void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCa qaabbMax += aabbMax; reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); } + */ + } @@ -716,17 +815,19 @@ void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNod //PCK: include #include +#if 0 //PCK: consts static const unsigned BVH_ALIGNMENT = 16; static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1; static const unsigned BVH_ALIGNMENT_BLOCKS = 2; - +#endif unsigned int btQuantizedBvh::getAlignmentSerializationPadding() { - return BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; + // I changed this to 0 since the extra padding is not needed or used. + return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; } unsigned btQuantizedBvh::calculateSerializeBufferSize() @@ -742,7 +843,7 @@ unsigned btQuantizedBvh::calculateSerializeBufferSize() bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) { - assert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); + btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); m_subtreeHeaderCount = m_SubtreeHeaders.size(); /* if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) @@ -829,6 +930,11 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe } } nodeData += sizeof(btQuantizedBvhNode) * nodeCount; + + // this clears the pointer in the member variable it doesn't really do anything to the data + // it does call the destructor on the contained objects, but they are all classes with no destructor defined + // so the memory (which is not freed) is left alone + targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(NULL, 0, 0); } else { @@ -859,6 +965,11 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe } } nodeData += sizeof(btOptimizedBvhNode) * nodeCount; + + // this clears the pointer in the member variable it doesn't really do anything to the data + // it does call the destructor on the contained objects, but they are all classes with no destructor defined + // so the memory (which is not freed) is left alone + targetBvh->m_contiguousNodes.initializeFromBuffer(NULL, 0, 0); } sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; @@ -896,12 +1007,23 @@ bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBuffe targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = (m_SubtreeHeaders[i].m_rootNodeIndex); targetBvh->m_SubtreeHeaders[i].m_subtreeSize = (m_SubtreeHeaders[i].m_subtreeSize); - targetBvh->m_SubtreeHeaders[i] = m_SubtreeHeaders[i]; + + // need to clear padding in destination buffer + targetBvh->m_SubtreeHeaders[i].m_padding[0] = 0; + targetBvh->m_SubtreeHeaders[i].m_padding[1] = 0; + targetBvh->m_SubtreeHeaders[i].m_padding[2] = 0; } } - nodeData += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; + // this clears the pointer in the member variable it doesn't really do anything to the data + // it does call the destructor on the contained objects, but they are all classes with no destructor defined + // so the memory (which is not freed) is left alone + targetBvh->m_SubtreeHeaders.initializeFromBuffer(NULL, 0, 0); + + // this wipes the virtual function table pointer at the start of the buffer for the class + *((void**)o_alignedDataBuffer) = NULL; + return true; } @@ -1015,11 +1137,12 @@ btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, un btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) : m_bvhAabbMin(self.m_bvhAabbMin), m_bvhAabbMax(self.m_bvhAabbMax), -m_bvhQuantization(self.m_bvhQuantization) +m_bvhQuantization(self.m_bvhQuantization), +m_bulletVersion(BT_BULLET_VERSION) { - } + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h index 8a149b533fa..ced457b6036 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -158,41 +158,43 @@ typedef btAlignedObjectArray BvhSubtreeInfoArray; ///It is recommended to use quantization for better performance and lower memory requirements. ATTRIBUTE_ALIGNED16(class) btQuantizedBvh { -protected: - - NodeArray m_leafNodes; - NodeArray m_contiguousNodes; - - QuantizedNodeArray m_quantizedLeafNodes; - - QuantizedNodeArray m_quantizedContiguousNodes; - - int m_curNodeIndex; - - - //quantization data - bool m_useQuantization; - btVector3 m_bvhAabbMin; - btVector3 m_bvhAabbMax; - btVector3 m_bvhQuantization; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - enum btTraversalMode { TRAVERSAL_STACKLESS = 0, TRAVERSAL_STACKLESS_CACHE_FRIENDLY, TRAVERSAL_RECURSIVE }; + protected: - btTraversalMode m_traversalMode; + + btVector3 m_bvhAabbMin; + btVector3 m_bvhAabbMax; + btVector3 m_bvhQuantization; + + int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess. + + int m_curNodeIndex; + //quantization data + bool m_useQuantization; + + + + NodeArray m_leafNodes; + NodeArray m_contiguousNodes; + QuantizedNodeArray m_quantizedLeafNodes; + QuantizedNodeArray m_quantizedContiguousNodes; + btTraversalMode m_traversalMode; BvhSubtreeInfoArray m_SubtreeHeaders; //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray int m_subtreeHeaderCount; + + + ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) ///this might be refactored into a virtual, it is usually not calculated at run-time @@ -296,6 +298,7 @@ protected: void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; + void walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; ///tree traversal designed for small-memory processors like PS3 SPU void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; @@ -307,30 +310,14 @@ protected: void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const; -#define USE_BANCHLESS 1 -#ifdef USE_BANCHLESS - //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) - SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const - { - return static_cast(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) - & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) - & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), - 1, 0)); - } -#else - SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const - { - bool overlap = true; - overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; - return overlap; - } -#endif //USE_BANCHLESS + void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + btQuantizedBvh(); virtual ~btQuantizedBvh(); @@ -363,7 +350,7 @@ public: btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization; ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly) - ///todo: double-check this + ///@todo: double-check this if (isMax) { out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1)); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index a57952ffa06..caed63db005 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -55,6 +55,7 @@ btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* o m_maxHandles = maxProxies; m_numHandles = 0; m_firstFreeHandle = 0; + m_LastHandleIndex = -1; { @@ -88,7 +89,7 @@ btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, btAssert(0); return 0; //should never happen, but don't let the game crash ;-) } - assert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]); + btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]); int newHandleIndex = allocHandle(); btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy); @@ -137,14 +138,32 @@ void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* } +void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); + aabbMin = sbp->m_aabbMin; + aabbMax = sbp->m_aabbMax; +} + void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/) { btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); - sbp->m_min = aabbMin; - sbp->m_max = aabbMax; + sbp->m_aabbMin = aabbMin; + sbp->m_aabbMax = aabbMax; } - +void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) +{ + for (int i=0; i <= m_LastHandleIndex; i++) + { + btSimpleBroadphaseProxy* proxy = &m_pHandles[i]; + if(!proxy->m_clientObject) + { + continue; + } + rayCallback.process(proxy); + } +} @@ -154,9 +173,9 @@ void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbM bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1) { - return proxy0->m_min[0] <= proxy1->m_max[0] && proxy1->m_min[0] <= proxy0->m_max[0] && - proxy0->m_min[1] <= proxy1->m_max[1] && proxy1->m_min[1] <= proxy0->m_max[1] && - proxy0->m_min[2] <= proxy1->m_max[2] && proxy1->m_min[2] <= proxy0->m_max[2]; + return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && + proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] && + proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2]; } @@ -176,18 +195,25 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { //first check for new overlapping pairs int i,j; - if (m_numHandles >= 0) { - - for (i=0;im_clientObject) + { + continue; + } + new_largest_index = i; + for (j=i+1; j <= m_LastHandleIndex; j++) { btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j]; btAssert(proxy0 != proxy1); + if(!proxy1->m_clientObject) + { + continue; + } btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); @@ -211,6 +237,8 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) } } + m_LastHandleIndex = new_largest_index; + if (m_ownsPairCache && m_pairCache->hasDeferredRemoval()) { @@ -296,5 +324,7 @@ bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseP return aabbOverlap(p0,p1); } - - +void btSimpleBroadphase::resetPool(btDispatcher* dispatcher) +{ + //not yet +} diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index e2ebb825725..cc7613bf6a0 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -22,8 +22,6 @@ subject to the following restrictions: struct btSimpleBroadphaseProxy : public btBroadphaseProxy { - btVector3 m_min; - btVector3 m_max; int m_nextFree; // int m_handleId; @@ -31,9 +29,8 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy btSimpleBroadphaseProxy() {}; - btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy) - :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy), - m_min(minpt),m_max(maxpt) + btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy) + :btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy) { (void)shapeType; } @@ -56,6 +53,7 @@ protected: int m_numHandles; // number of active handles int m_maxHandles; // max number of handles + int m_LastHandleIndex; btSimpleBroadphaseProxy* m_pHandles; // handles pool @@ -68,6 +66,10 @@ protected: int freeHandle = m_firstFreeHandle; m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree(); m_numHandles++; + if(freeHandle > m_LastHandleIndex) + { + m_LastHandleIndex = freeHandle; + } return freeHandle; } @@ -75,10 +77,15 @@ protected: { int handle = int(proxy-m_pHandles); btAssert(handle >= 0 && handle < m_maxHandles); - + if(handle == m_LastHandleIndex) + { + m_LastHandleIndex--; + } proxy->SetNextFree(m_firstFreeHandle); m_firstFreeHandle = handle; + proxy->m_clientObject = 0; + m_numHandles--; } @@ -95,6 +102,15 @@ protected: return proxy0; } + inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const + { + const btSimpleBroadphaseProxy* proxy0 = static_cast(proxy); + return proxy0; + } + + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(btDispatcher* dispatcher); + void validate(); @@ -117,6 +133,9 @@ public: virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher); + virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); btOverlappingPairCache* getOverlappingPairCache() { diff --git a/extern/bullet2/src/BulletCollision/CMakeLists.txt b/extern/bullet2/src/BulletCollision/CMakeLists.txt index d77ca6444c7..4b4304f43b0 100644 --- a/extern/bullet2/src/BulletCollision/CMakeLists.txt +++ b/extern/bullet2/src/BulletCollision/CMakeLists.txt @@ -1,153 +1,234 @@ - -INCLUDE_DIRECTORIES( -${BULLET_PHYSICS_SOURCE_DIR}/src } +INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src } ) + +SET(BulletCollision_SRCS + BroadphaseCollision/btAxisSweep3.cpp + BroadphaseCollision/btBroadphaseProxy.cpp + BroadphaseCollision/btCollisionAlgorithm.cpp + BroadphaseCollision/btDispatcher.cpp + BroadphaseCollision/btDbvtBroadphase.cpp + BroadphaseCollision/btDbvt.cpp + BroadphaseCollision/btMultiSapBroadphase.cpp + BroadphaseCollision/btOverlappingPairCache.cpp + BroadphaseCollision/btQuantizedBvh.cpp + BroadphaseCollision/btSimpleBroadphase.cpp + CollisionDispatch/btActivatingCollisionAlgorithm.cpp + CollisionDispatch/btCollisionDispatcher.cpp + CollisionDispatch/btCollisionObject.cpp + CollisionDispatch/btCollisionWorld.cpp + CollisionDispatch/btCompoundCollisionAlgorithm.cpp + CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp + CollisionDispatch/btDefaultCollisionConfiguration.cpp + CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp + CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp + CollisionDispatch/btBoxBoxDetector.cpp + CollisionDispatch/btGhostObject.cpp + CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp + CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp + CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp + CollisionDispatch/btConvexConvexAlgorithm.cpp + CollisionDispatch/btEmptyCollisionAlgorithm.cpp + CollisionDispatch/btManifoldResult.cpp + CollisionDispatch/btSimulationIslandManager.cpp + CollisionDispatch/btUnionFind.cpp + CollisionDispatch/SphereTriangleDetector.cpp + CollisionShapes/btBoxShape.cpp + CollisionShapes/btBvhTriangleMeshShape.cpp + CollisionShapes/btCapsuleShape.cpp + CollisionShapes/btCollisionShape.cpp + CollisionShapes/btCompoundShape.cpp + CollisionShapes/btConcaveShape.cpp + CollisionShapes/btConeShape.cpp + CollisionShapes/btConvexHullShape.cpp + CollisionShapes/btConvexPointCloudShape.cpp + CollisionShapes/btConvexShape.cpp + CollisionShapes/btConvexInternalShape.cpp + CollisionShapes/btConvexTriangleMeshShape.cpp + CollisionShapes/btCylinderShape.cpp + CollisionShapes/btEmptyShape.cpp + CollisionShapes/btHeightfieldTerrainShape.cpp + CollisionShapes/btMinkowskiSumShape.cpp + CollisionShapes/btMultimaterialTriangleMeshShape.cpp + CollisionShapes/btMultiSphereShape.cpp + CollisionShapes/btOptimizedBvh.cpp + CollisionShapes/btPolyhedralConvexShape.cpp + CollisionShapes/btScaledBvhTriangleMeshShape.cpp + CollisionShapes/btTetrahedronShape.cpp + CollisionShapes/btSphereShape.cpp + CollisionShapes/btShapeHull.cpp + CollisionShapes/btStaticPlaneShape.cpp + CollisionShapes/btStridingMeshInterface.cpp + CollisionShapes/btTriangleCallback.cpp + CollisionShapes/btTriangleBuffer.cpp + CollisionShapes/btTriangleIndexVertexArray.cpp + CollisionShapes/btTriangleIndexVertexMaterialArray.cpp + CollisionShapes/btTriangleMesh.cpp + CollisionShapes/btTriangleMeshShape.cpp + CollisionShapes/btUniformScalingShape.cpp + Gimpact/btContactProcessing.cpp + Gimpact/btGImpactShape.cpp + Gimpact/gim_contact.cpp + Gimpact/btGImpactBvh.cpp + Gimpact/btGenericPoolAllocator.cpp + Gimpact/gim_memory.cpp + Gimpact/btGImpactCollisionAlgorithm.cpp + Gimpact/btTriangleShapeEx.cpp + Gimpact/gim_tri_collision.cpp + Gimpact/btGImpactQuantizedBvh.cpp + Gimpact/gim_box_set.cpp + NarrowPhaseCollision/btContinuousConvexCollision.cpp + NarrowPhaseCollision/btGjkEpa2.cpp + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp + NarrowPhaseCollision/btConvexCast.cpp + NarrowPhaseCollision/btGjkConvexCast.cpp + NarrowPhaseCollision/btGjkPairDetector.cpp + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp + NarrowPhaseCollision/btPersistentManifold.cpp + NarrowPhaseCollision/btRaycastCallback.cpp + NarrowPhaseCollision/btSubSimplexConvexCast.cpp + NarrowPhaseCollision/btVoronoiSimplexSolver.cpp ) - -ADD_LIBRARY(LibBulletCollision - BroadphaseCollision/btAxisSweep3.cpp - BroadphaseCollision/btAxisSweep3.h - BroadphaseCollision/btBroadphaseProxy.cpp - BroadphaseCollision/btBroadphaseProxy.h - BroadphaseCollision/btCollisionAlgorithm.cpp - BroadphaseCollision/btCollisionAlgorithm.h - BroadphaseCollision/btDispatcher.cpp - BroadphaseCollision/btDispatcher.h - BroadphaseCollision/btDbvtBroadphase.cpp - BroadphaseCollision/btDbvtBroadphase.h - BroadphaseCollision/btDbvt.cpp - BroadphaseCollision/btDbvt.h - BroadphaseCollision/btMultiSapBroadphase.cpp - BroadphaseCollision/btMultiSapBroadphase.h - BroadphaseCollision/btOverlappingPairCache.cpp - BroadphaseCollision/btOverlappingPairCache.h - BroadphaseCollision/btOverlappingPairCallback.h - BroadphaseCollision/btQuantizedBvh.cpp - BroadphaseCollision/btQuantizedBvh.h - BroadphaseCollision/btSimpleBroadphase.cpp - BroadphaseCollision/btSimpleBroadphase.h - CollisionDispatch/btCollisionDispatcher.cpp - CollisionDispatch/btCollisionDispatcher.h - CollisionDispatch/btCollisionObject.cpp - CollisionDispatch/btCollisionObject.h - CollisionDispatch/btCollisionWorld.cpp - CollisionDispatch/btCollisionWorld.h - CollisionDispatch/btCompoundCollisionAlgorithm.cpp - CollisionDispatch/btCompoundCollisionAlgorithm.h - CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp - CollisionDispatch/btConvexConcaveCollisionAlgorithm.h - CollisionDispatch/btDefaultCollisionConfiguration.cpp - CollisionDispatch/btDefaultCollisionConfiguration.h - CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp - CollisionDispatch/btSphereSphereCollisionAlgorithm.h - CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp - CollisionDispatch/btBoxBoxCollisionAlgorithm.h - CollisionDispatch/btBoxBoxDetector.cpp - CollisionDispatch/btBoxBoxDetector.h - CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp - CollisionDispatch/btSphereBoxCollisionAlgorithm.h - CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp - CollisionDispatch/btConvexPlaneCollisionAlgorithm.h - CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp - CollisionDispatch/btSphereTriangleCollisionAlgorithm.h - CollisionDispatch/btConvexConvexAlgorithm.cpp - CollisionDispatch/btConvexConvexAlgorithm.h - CollisionDispatch/btEmptyCollisionAlgorithm.cpp - CollisionDispatch/btEmptyCollisionAlgorithm.h - CollisionDispatch/btManifoldResult.cpp - CollisionDispatch/btManifoldResult.h - CollisionDispatch/btSimulationIslandManager.cpp - CollisionDispatch/btSimulationIslandManager.h - CollisionDispatch/btUnionFind.cpp - CollisionDispatch/btUnionFind.h - CollisionDispatch/SphereTriangleDetector.cpp - CollisionDispatch/SphereTriangleDetector.h - CollisionShapes/btBoxShape.cpp - CollisionShapes/btBoxShape.h - CollisionShapes/btBvhTriangleMeshShape.cpp - CollisionShapes/btBvhTriangleMeshShape.h - CollisionShapes/btCapsuleShape.cpp - CollisionShapes/btCapsuleShape.h - CollisionShapes/btCollisionShape.cpp - CollisionShapes/btCollisionShape.h - CollisionShapes/btCompoundShape.cpp - CollisionShapes/btCompoundShape.h - CollisionShapes/btConcaveShape.cpp - CollisionShapes/btConcaveShape.h - CollisionShapes/btConeShape.cpp - CollisionShapes/btConeShape.h - CollisionShapes/btConvexHullShape.cpp - CollisionShapes/btConvexHullShape.h - CollisionShapes/btConvexShape.cpp - CollisionShapes/btConvexShape.h - CollisionShapes/btConvexInternalShape.cpp - CollisionShapes/btConvexInternalShape.h - CollisionShapes/btConvexTriangleMeshShape.cpp - CollisionShapes/btConvexTriangleMeshShape.h - CollisionShapes/btCylinderShape.cpp - CollisionShapes/btCylinderShape.h - CollisionShapes/btEmptyShape.cpp - CollisionShapes/btEmptyShape.h - CollisionShapes/btHeightfieldTerrainShape.cpp - CollisionShapes/btHeightfieldTerrainShape.h - CollisionShapes/btMinkowskiSumShape.cpp - CollisionShapes/btMinkowskiSumShape.h - CollisionShapes/btMaterial.h - CollisionShapes/btMultimaterialTriangleMeshShape.cpp - CollisionShapes/btMultimaterialTriangleMeshShape.h - CollisionShapes/btMultiSphereShape.cpp - CollisionShapes/btMultiSphereShape.h - CollisionShapes/btOptimizedBvh.cpp - CollisionShapes/btOptimizedBvh.h - CollisionShapes/btPolyhedralConvexShape.cpp - CollisionShapes/btPolyhedralConvexShape.h - CollisionShapes/btScaledBvhTriangleMeshShape.cpp - CollisionShapes/btScaledBvhTriangleMeshShape.h - CollisionShapes/btTetrahedronShape.cpp - CollisionShapes/btTetrahedronShape.h - CollisionShapes/btSphereShape.cpp - CollisionShapes/btSphereShape.h - CollisionShapes/btShapeHull.h - CollisionShapes/btShapeHull.cpp - CollisionShapes/btStaticPlaneShape.cpp - CollisionShapes/btStaticPlaneShape.h - CollisionShapes/btStridingMeshInterface.cpp - CollisionShapes/btStridingMeshInterface.h - CollisionShapes/btTriangleCallback.cpp - CollisionShapes/btTriangleCallback.h - CollisionShapes/btTriangleBuffer.cpp - CollisionShapes/btTriangleBuffer.h - CollisionShapes/btTriangleIndexVertexArray.cpp - CollisionShapes/btTriangleIndexVertexArray.h - CollisionShapes/btTriangleIndexVertexMaterialArray.h - CollisionShapes/btTriangleIndexVertexMaterialArray.cpp - CollisionShapes/btTriangleMesh.cpp - CollisionShapes/btTriangleMesh.h - CollisionShapes/btTriangleMeshShape.cpp - CollisionShapes/btTriangleMeshShape.h - CollisionShapes/btUniformScalingShape.cpp - CollisionShapes/btUniformScalingShape.h - NarrowPhaseCollision/btContinuousConvexCollision.cpp - NarrowPhaseCollision/btContinuousConvexCollision.h - NarrowPhaseCollision/btGjkEpa.cpp - NarrowPhaseCollision/btGjkEpa.h - NarrowPhaseCollision/btGjkEpa2.cpp - NarrowPhaseCollision/btGjkEpa2.h - NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp - NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h - NarrowPhaseCollision/btConvexCast.cpp - NarrowPhaseCollision/btConvexCast.h - NarrowPhaseCollision/btGjkConvexCast.cpp - NarrowPhaseCollision/btGjkConvexCast.h - NarrowPhaseCollision/btGjkPairDetector.cpp - NarrowPhaseCollision/btGjkPairDetector.h - NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp - NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h - NarrowPhaseCollision/btPersistentManifold.cpp - NarrowPhaseCollision/btPersistentManifold.h - NarrowPhaseCollision/btRaycastCallback.cpp - NarrowPhaseCollision/btRaycastCallback.h - NarrowPhaseCollision/btSubSimplexConvexCast.cpp - NarrowPhaseCollision/btSubSimplexConvexCast.h - NarrowPhaseCollision/btVoronoiSimplexSolver.cpp - NarrowPhaseCollision/btVoronoiSimplexSolver.h + +SET(Root_HDRS + ../btBulletCollisionCommon.h ) +SET(BroadphaseCollision_HDRS + BroadphaseCollision/btAxisSweep3.h + BroadphaseCollision/btBroadphaseInterface.h + BroadphaseCollision/btBroadphaseProxy.h + BroadphaseCollision/btCollisionAlgorithm.h + BroadphaseCollision/btDispatcher.h + BroadphaseCollision/btDbvtBroadphase.h + BroadphaseCollision/btDbvt.h + BroadphaseCollision/btMultiSapBroadphase.h + BroadphaseCollision/btOverlappingPairCache.h + BroadphaseCollision/btOverlappingPairCallback.h + BroadphaseCollision/btQuantizedBvh.h + BroadphaseCollision/btSimpleBroadphase.h +) +SET(CollisionDispatch_HDRS + CollisionDispatch/btActivatingCollisionAlgorithm.h + CollisionDispatch/btCollisionConfiguration.h + CollisionDispatch/btCollisionCreateFunc.h + CollisionDispatch/btCollisionDispatcher.h + CollisionDispatch/btCollisionObject.h + CollisionDispatch/btCollisionWorld.h + CollisionDispatch/btCompoundCollisionAlgorithm.h + CollisionDispatch/btConvexConcaveCollisionAlgorithm.h + CollisionDispatch/btDefaultCollisionConfiguration.h + CollisionDispatch/btSphereSphereCollisionAlgorithm.h + CollisionDispatch/btBoxBoxCollisionAlgorithm.h + CollisionDispatch/btBoxBoxDetector.h + CollisionDispatch/btGhostObject.h + CollisionDispatch/btSphereBoxCollisionAlgorithm.h + CollisionDispatch/btConvexPlaneCollisionAlgorithm.h + CollisionDispatch/btSphereTriangleCollisionAlgorithm.h + CollisionDispatch/btConvexConvexAlgorithm.h + CollisionDispatch/btEmptyCollisionAlgorithm.h + CollisionDispatch/btManifoldResult.h + CollisionDispatch/btSimulationIslandManager.h + CollisionDispatch/btUnionFind.h + CollisionDispatch/SphereTriangleDetector.h +) +SET(CollisionShapes_HDRS + CollisionShapes/btBoxShape.h + CollisionShapes/btBvhTriangleMeshShape.h + CollisionShapes/btCapsuleShape.h + CollisionShapes/btCollisionMargin + CollisionShapes/btCollisionShape.h + CollisionShapes/btCompoundShape.h + CollisionShapes/btConcaveShape.h + CollisionShapes/btConeShape.h + CollisionShapes/btConvexHullShape.h + CollisionShapes/btConvexPointCloudShape.h + CollisionShapes/btConvexShape.h + CollisionShapes/btConvexInternalShape.h + CollisionShapes/btConvexTriangleMeshShape.h + CollisionShapes/btCylinderShape.h + CollisionShapes/btEmptyShape.h + CollisionShapes/btHeightfieldTerrainShape.h + CollisionShapes/btMinkowskiSumShape.h + CollisionShapes/btMaterial.h + CollisionShapes/btMultimaterialTriangleMeshShape.h + CollisionShapes/btMultiSphereShape.h + CollisionShapes/btOptimizedBvh.h + CollisionShapes/btPolyhedralConvexShape.h + CollisionShapes/btScaledBvhTriangleMeshShape.h + CollisionShapes/btTetrahedronShape.h + CollisionShapes/btSphereShape.h + CollisionShapes/btShapeHull.h + CollisionShapes/btStaticPlaneShape.h + CollisionShapes/btStridingMeshInterface.h + CollisionShapes/btTriangleCallback.h + CollisionShapes/btTriangleBuffer.h + CollisionShapes/btTriangleIndexVertexArray.h + CollisionShapes/btTriangleIndexVertexMaterialArray.h + CollisionShapes/btTriangleMesh.h + CollisionShapes/btTriangleMeshShape.h + CollisionShapes/btUniformScalingShape.h +) +SET(Gimpact_HDRS + Gimpact/btGImpactShape.h + Gimpact/gim_contact.h + Gimpact/btGImpactBvh.h + Gimpact/btGenericPoolAllocator.h + Gimpact/gim_memory.h + Gimpact/btGImpactCollisionAlgorithm.h + Gimpact/btTriangleShapeEx.h + Gimpact/gim_tri_collision.h + Gimpact/btGImpactQuantizedBvh.h + Gimpact/gim_box_set.h +) +SET(NarrowPhaseCollision_HDRS + NarrowPhaseCollision/btContinuousConvexCollision.h + NarrowPhaseCollision/btConvexCast.h + NarrowPhaseCollision/btConvexPenetrationDepthSolver.h + NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h + NarrowPhaseCollision/btGjkConvexCast.h + NarrowPhaseCollision/btGjkEpa2.h + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h + NarrowPhaseCollision/btGjkPairDetector.h + NarrowPhaseCollision/btManifoldPoint.h + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h + NarrowPhaseCollision/btPersistentManifold.h + NarrowPhaseCollision/btPointCollector.h + NarrowPhaseCollision/btRaycastCallback.h + NarrowPhaseCollision/btSimplexSolverInterface.h + NarrowPhaseCollision/btSubSimplexConvexCast.h + NarrowPhaseCollision/btVoronoiSimplexSolver.h +) + +SET(BulletCollision_HDRS + ${Root_HDRS} + ${BroadphaseCollision_HDRS} + ${CollisionDispatch_HDRS} + ${CollisionShapes_HDRS} + ${Gimpact_HDRS} + ${NarrowPhaseCollision_HDRS} +) + + +ADD_LIBRARY(BulletCollision ${BulletCollision_SRCS} ${BulletCollision_HDRS}) +SET_TARGET_PROPERTIES(BulletCollision PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletCollision PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletCollision LinearMath) +ENDIF (BUILD_SHARED_LIBS) + +#INSTALL of other files requires CMake 2.6 +IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + INSTALL(TARGETS BulletCollision DESTINATION lib) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") +ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + +IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletCollision PROPERTIES FRAMEWORK true) + + SET_TARGET_PROPERTIES(BulletCollision PROPERTIES PUBLIC_HEADER "${Root_HDRS}") + # Have to list out sub-directories manually: + SET_PROPERTY(SOURCE ${BroadphaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadphaseCollision) + SET_PROPERTY(SOURCE ${CollisionDispatch_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/CollisionDispatch) + SET_PROPERTY(SOURCE ${CollisionShapes_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/CollisionShapes) + SET_PROPERTY(SOURCE ${Gimpact_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Gimpact) + SET_PROPERTY(SOURCE ${NarrowPhaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/NarrowPhaseCollision) + +ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index f6c1e32ac7c..9a749a03793 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -19,9 +19,10 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btSphereShape.h" -SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle) +SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) :m_sphere(sphere), -m_triangle(triangle) +m_triangle(triangle), +m_contactBreakingThreshold(contactBreakingThreshold) { } @@ -40,7 +41,7 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res //move sphere into triangle space btTransform sphereInTr = transformB.inverseTimes(transformA); - if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact)) + if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) { if (swapResults) { @@ -93,7 +94,7 @@ bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* ve } ///combined discrete/continuous sphere-triangle -bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact) +bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) { const btVector3* vertices = &m_triangle->getVertexPtr(0); @@ -115,10 +116,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po normal *= btScalar(-1.); } - ///todo: move this gContactBreakingThreshold into a proper structure - extern btScalar gContactBreakingThreshold; - - btScalar contactMargin = gContactBreakingThreshold; + btScalar contactMargin = contactBreakingThreshold; bool isInsideContactPlane = distanceFromPlane < r + contactMargin; bool isInsideShellPlane = distanceFromPlane < r; @@ -140,8 +138,8 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po btVector3 nearestOnEdge; for (int i = 0; i < m_triangle->getNumEdges(); i++) { - btPoint3 pa; - btPoint3 pb; + btVector3 pa; + btVector3 pb; m_triangle->getEdge(i,pa,pb); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 26dabaa480e..981bd54e76c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define SPHERE_TRIANGLE_DETECTOR_H #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btPoint3.h" + class btSphereShape; @@ -30,19 +30,19 @@ struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface { virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); - SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle); + SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold); virtual ~SphereTriangleDetector() {}; private: - bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact); + bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold); bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ); bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal); btSphereShape* m_sphere; btTriangleShape* m_triangle; - + btScalar m_contactBreakingThreshold; }; #endif //SPHERE_TRIANGLE_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp new file mode 100644 index 00000000000..7e5da6c5872 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp @@ -0,0 +1,47 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +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 "btActivatingCollisionAlgorithm.h" +#include "btCollisionDispatcher.h" +#include "btCollisionObject.h" + +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) +:btCollisionAlgorithm(ci) +//, +//m_colObj0(0), +//m_colObj1(0) +{ +} +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1) +:btCollisionAlgorithm(ci) +//, +//m_colObj0(0), +//m_colObj1(0) +{ +// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) +// { +// m_colObj0 = colObj0; +// m_colObj1 = colObj1; +// +// m_colObj0->activate(); +// m_colObj1->activate(); +// } +} + +btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm() +{ +// m_colObj0->activate(); +// m_colObj1->activate(); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h new file mode 100644 index 00000000000..25fe088942d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -0,0 +1,36 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +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_ACTIVATING_COLLISION_ALGORITHM_H +#define __BT_ACTIVATING_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + +///This class is not enabled yet (work-in-progress) to more aggressively activate objects. +class btActivatingCollisionAlgorithm : public btCollisionAlgorithm +{ +// btCollisionObject* m_colObj0; +// btCollisionObject* m_colObj1; + +public: + + btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); + + btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1); + + virtual ~btActivatingCollisionAlgorithm(); + +}; +#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp index cd0c028012c..d3342c547b5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: #define USE_PERSISTENT_CONTACTS 1 btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) -: btCollisionAlgorithm(ci), +: btActivatingCollisionAlgorithm(ci,obj0,obj1), m_ownManifold(false), m_manifoldPtr(mf) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h index 35afaf175a1..e7d2cc25c22 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef BOX_BOX__COLLISION_ALGORITHM_H #define BOX_BOX__COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" @@ -24,14 +24,14 @@ subject to the following restrictions: class btPersistentManifold; ///box-box collision detection -class btBoxBoxCollisionAlgorithm : public btCollisionAlgorithm +class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm { bool m_ownManifold; btPersistentManifold* m_manifoldPtr; public: btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btCollisionAlgorithm(ci) {} + : btActivatingCollisionAlgorithm(ci) {} virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index 45ebff5dc45..31353f1b2c4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -207,7 +207,13 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) cy += q*(p[i*2+1]+p[i*2+3]); } q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; - a = 1.f/(btScalar(3.0)*(a+q)); + if (btFabs(a+q) > SIMD_EPSILON) + { + a = 1.f/(btScalar(3.0)*(a+q)); + } else + { + a=1e30f; + } cx = a*(cx + q*(p[n*2-2]+p[0])); cy = a*(cy + q*(p[n*2-1]+p[1])); } @@ -226,9 +232,9 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) a = btScalar(j)*(2*M__PI/m) + A[i0]; if (a > M__PI) a -= 2*M__PI; btScalar maxdiff=1e9,diff; -#if defined(DEBUG) || defined (_DEBUG) - *iret = i0; // iret is not allowed to keep this value -#endif + + *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 + for (i=0; i btCollisionObjectArray; class btCollisionAlgorithm; class btCollisionObject; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index a031a9f9784..e6ff2130aad 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -52,12 +52,12 @@ btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisio for (int j=0;jgetCollisionAlgorithmCreateFunc(i,j); - assert(m_doubleDispatch[i][j]); + btAssert(m_doubleDispatch[i][j]); } } -}; +} void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) @@ -78,7 +78,13 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) btCollisionObject* body0 = (btCollisionObject*)b0; btCollisionObject* body1 = (btCollisionObject*)b1; - + + //test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold' + //btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold())); + btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()); + + btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold()); + void* mem = 0; if (m_persistentManifoldPoolAllocator->getFreeCount()) @@ -89,7 +95,7 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) mem = btAlignedAlloc(sizeof(btPersistentManifold),16); } - btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0); + btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold); manifold->m_index1a = m_manifoldsPtr.size(); m_manifoldsPtr.push_back(manifold); @@ -144,7 +150,6 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo - bool btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1) { //here you can do filtering @@ -158,8 +163,8 @@ bool btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionOb bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1) { - assert(body0); - assert(body1); + btAssert(body0); + btAssert(body1); bool needsCollision = true; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 846c9b9b989..285b8f174e4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -19,6 +19,7 @@ subject to the following restrictions: btCollisionObject::btCollisionObject() : m_anisotropicFriction(1.f,1.f,1.f), m_hasAnisotropicFriction(false), + m_contactProcessingThreshold(0.f), m_broadphaseHandle(0), m_collisionShape(0), m_rootCollisionShape(0), diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 8442868cf89..0d5b7886443 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -29,8 +29,11 @@ struct btBroadphaseProxy; class btCollisionShape; #include "LinearMath/btMotionState.h" #include "LinearMath/btAlignedAllocator.h" +#include "LinearMath/btAlignedObjectArray.h" +typedef btAlignedObjectArray btCollisionObjectArray; + /// btCollisionObject can be used to manage collision detection objects. /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. @@ -49,8 +52,10 @@ protected: //without destroying the continuous interpolated motion (which uses this interpolation velocities) btVector3 m_interpolationLinearVelocity; btVector3 m_interpolationAngularVelocity; + btVector3 m_anisotropicFriction; - bool m_hasAnisotropicFriction; + bool m_hasAnisotropicFriction; + btScalar m_contactProcessingThreshold; btBroadphaseProxy* m_broadphaseHandle; btCollisionShape* m_collisionShape; @@ -74,7 +79,7 @@ protected: ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer void* m_userObjectPointer; - ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody etc. + ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc. ///do not assign your own m_internalType unless you write a new dynamics object class. int m_internalType; @@ -106,14 +111,19 @@ public: CF_STATIC_OBJECT= 1, CF_KINEMATIC_OBJECT= 2, CF_NO_CONTACT_RESPONSE = 4, - CF_CUSTOM_MATERIAL_CALLBACK = 8//this allows per-triangle material (friction/restitution) + CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) + CF_CHARACTER_OBJECT = 16 }; enum CollisionObjectTypes { CO_COLLISION_OBJECT =1, CO_RIGID_BODY, - CO_SOFT_BODY + ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter + ///It is useful for collision sensors, explosion objects, character controller etc. + CO_GHOST_OBJECT, + CO_SOFT_BODY, + CO_HF_FLUID }; SIMD_FORCE_INLINE bool mergesSimulationIslands() const @@ -136,6 +146,16 @@ public: return m_hasAnisotropicFriction; } + ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default. + ///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges + void setContactProcessingThreshold( btScalar contactProcessingThreshold) + { + m_contactProcessingThreshold = contactProcessingThreshold; + } + btScalar getContactProcessingThreshold() const + { + return m_contactProcessingThreshold; + } SIMD_FORCE_INLINE bool isStaticObject() const { return (m_collisionFlags & CF_STATIC_OBJECT) != 0; @@ -193,7 +213,7 @@ public: m_collisionShape = collisionShape; } - int getActivationState() const { return m_activationState1;} + SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;} void setActivationState(int newState); @@ -210,7 +230,7 @@ public: void activate(bool forceActivation = false); - inline bool isActive() const + SIMD_FORCE_INLINE bool isActive() const { return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } @@ -254,12 +274,12 @@ public: } - btBroadphaseProxy* getBroadphaseHandle() + SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle() { return m_broadphaseHandle; } - const btBroadphaseProxy* getBroadphaseHandle() const + SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const { return m_broadphaseHandle; } @@ -305,7 +325,7 @@ public: return m_interpolationAngularVelocity; } - const int getIslandTag() const + SIMD_FORCE_INLINE int getIslandTag() const { return m_islandTag1; } @@ -315,7 +335,7 @@ public: m_islandTag1 = tag; } - const int getCompanionId() const + SIMD_FORCE_INLINE int getCompanionId() const { return m_companionId; } @@ -325,7 +345,7 @@ public: m_companionId = id; } - const btScalar getHitFraction() const + SIMD_FORCE_INLINE btScalar getHitFraction() const { return m_hitFraction; } @@ -336,7 +356,7 @@ public: } - const int getCollisionFlags() const + SIMD_FORCE_INLINE int getCollisionFlags() const { return m_collisionFlags; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index d8674a320a7..10e880e2523 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -32,6 +32,9 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "LinearMath/btStackAlloc.h" +//#define USE_BRUTEFORCE_RAYBROADPHASE 1 +//RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest +//#define RECALCULATE_AABB_RAYCAST 1 //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor) #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -66,6 +69,7 @@ btCollisionWorld::~btCollisionWorld() // getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); getBroadphase()->destroyProxy(bp,m_dispatcher1); + collisionObject->setBroadphaseHandle(0); } } @@ -111,6 +115,41 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho +} + + + +void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) +{ + btVector3 minAabb,maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + //need to increase the aabb for contact thresholds + btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + minAabb -= contactThreshold; + maxAabb += contactThreshold; + + btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; + + //moving objects should be moderately sized, probably something wrong if not + if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) + { + bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); + } else + { + //something went wrong, investigate + //this assert is unwanted in 3D modelers (danger of loosing work) + colObj->setActivationState(DISABLE_SIMULATION); + + static bool reportMe = true; + if (reportMe && m_debugDrawer) + { + reportMe = false; + m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); + m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); + m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); + m_debugDrawer->reportErrorWarning("Thanks.\n"); + } + } } void btCollisionWorld::updateAabbs() @@ -125,38 +164,9 @@ void btCollisionWorld::updateAabbs() //only update aabb of active objects if (colObj->isActive()) { - btPoint3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - //need to increase the aabb for contact thresholds - btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); - minAabb -= contactThreshold; - maxAabb += contactThreshold; - - btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; - - //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) - { - bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } else - { - //something went wrong, investigate - //this assert is unwanted in 3D modelers (danger of loosing work) - colObj->setActivationState(DISABLE_SIMULATION); - - static bool reportMe = true; - if (reportMe && m_debugDrawer) - { - reportMe = false; - m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); - m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); - m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); - m_debugDrawer->reportErrorWarning("Thanks.\n"); - } - } + updateSingleAabb(colObj); } } - } @@ -226,6 +236,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra if (collisionShape->isConvex()) { +// BT_PROFILE("rayTestConvex"); btConvexCast::CastResult castResult; castResult.m_fraction = resultCallback.m_closestHitFraction; @@ -269,6 +280,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra } else { if (collisionShape->isConcave()) { +// BT_PROFILE("rayTestConcave"); if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { ///optimized version for btBvhTriangleMeshShape @@ -286,7 +298,8 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): - btTriangleRaycastCallback(from,to), + //@BP Mod + btTriangleRaycastCallback(from,to, resultCallback->m_flags), m_resultCallback(resultCallback), m_collisionObject(collisionObject), m_triangleMesh(triangleMesh) @@ -317,7 +330,8 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); } else { - btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape; + //generic (slower) case + btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); @@ -330,11 +344,12 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra { btCollisionWorld::RayResultCallback* m_resultCallback; btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; + btConcaveShape* m_triangleMesh; BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): - btTriangleRaycastCallback(from,to), + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh): + //@BP Mod + btTriangleRaycastCallback(from,to, resultCallback->m_flags), m_resultCallback(resultCallback), m_collisionObject(collisionObject), m_triangleMesh(triangleMesh) @@ -363,7 +378,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra }; - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; @@ -371,10 +386,11 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax(rayToLocal); - triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); } } else { - //todo: use AABB tree or other BVH acceleration structure! +// BT_PROFILE("rayTestCompound"); + ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt if (collisionShape->isCompound()) { const btCompoundShape* compoundShape = static_cast(collisionShape); @@ -408,9 +424,10 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { if (collisionShape->isConvex()) { + //BT_PROFILE("convexSweepConvex"); btConvexCast::CastResult castResult; castResult.m_allowedPenetration = allowedPenetration; - castResult.m_fraction = btScalar(1.);//?? + castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//?? btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; @@ -452,6 +469,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { + //BT_PROFILE("convexSweepbtBvhTriangleMesh"); btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); @@ -508,7 +526,8 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); } else { - btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + //BT_PROFILE("convexSweepConcave"); + btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); @@ -520,10 +539,10 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { btCollisionWorld::ConvexResultCallback* m_resultCallback; btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; + btConcaveShape* m_triangleMesh; BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, - btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): + btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), m_resultCallback(resultCallback), m_collisionObject(collisionObject), @@ -556,7 +575,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt }; - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 boxMinLocal, boxMaxLocal; castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); @@ -567,12 +586,13 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt rayAabbMaxLocal.setMax(convexToLocal); rayAabbMinLocal += boxMinLocal; rayAabbMaxLocal += boxMaxLocal; - triangleMesh->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); } } else { - //todo: use AABB tree or other BVH acceleration structure! + ///@todo : use AABB tree or other BVH acceleration structure! if (collisionShape->isCompound()) { + BT_PROFILE("convexSweepCompound"); const btCompoundShape* compoundShape = static_cast(collisionShape); int i=0; for (i=0;igetNumChildShapes();i++) @@ -596,51 +616,173 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt } } -void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const + +struct btSingleRayCallback : public btBroadphaseRayCallback { + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; - btTransform rayFromTrans,rayToTrans; - rayFromTrans.setIdentity(); - rayFromTrans.setOrigin(rayFromWorld); - rayToTrans.setIdentity(); + const btCollisionWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; - rayToTrans.setOrigin(rayToWorld); + btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback) + :m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) + { + m_rayFromTrans.setIdentity(); + m_rayFromTrans.setOrigin(m_rayFromWorld); + m_rayToTrans.setIdentity(); + m_rayToTrans.setOrigin(m_rayToWorld); - /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD) + btVector3 rayDir = (rayToWorld-rayFromWorld); - int i; - for (i=0;i just set rayDirection[i] to INF/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); + + } + + + + virtual bool process(const btBroadphaseProxy* proxy) { ///terminate further ray tests, once the closestHitFraction reached zero - if (resultCallback.m_closestHitFraction == btScalar(0.f)) - break; + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - btCollisionObject* collisionObject= m_collisionObjects[i]; //only perform raycast if filterMask matches - if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; +#if 0 +#ifdef RECALCULATE_AABB btVector3 collisionObjectAabbMin,collisionObjectAabbMax; collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - - btScalar hitLambda = resultCallback.m_closestHitFraction; - btVector3 hitNormal; - if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) +#else + //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); + const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; + const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; +#endif +#endif + //btScalar hitLambda = m_resultCallback.m_closestHitFraction; + //culling already done by broadphase + //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) { - rayTestSingle(rayFromTrans,rayToTrans, + m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, collisionObject, collisionObject->getCollisionShape(), collisionObject->getWorldTransform(), - resultCallback); + m_resultCallback); } } - + return true; } +}; + +void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const +{ + BT_PROFILE("rayTest"); + /// use the broadphase to accelerate the search for objects, based on their aabb + /// and for each object with ray-aabb overlap, perform an exact ray test + btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); + +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); +#else + for (int i=0;igetNumCollisionObjects();i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE } -void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback) const + +struct btSingleSweepCallback : public btBroadphaseRayCallback { + + btTransform m_convexFromTrans; + btTransform m_convexToTrans; + btVector3 m_hitNormal; + const btCollisionWorld* m_world; + btCollisionWorld::ConvexResultCallback& m_resultCallback; + btScalar m_allowedCcdPenetration; + const btConvexShape* m_castShape; + + + btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration) + :m_convexFromTrans(convexFromTrans), + m_convexToTrans(convexToTrans), + m_world(world), + m_resultCallback(resultCallback), + m_allowedCcdPenetration(allowedPenetration), + m_castShape(castShape) + { + btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin()); + btVector3 rayDir = unnormalizedRayDir.normalized(); + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(unnormalizedRayDir); + + } + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further convex sweep tests, once the closestHitFraction reached zero + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + + //only perform raycast if filterMask matches + if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback, + m_allowedCcdPenetration); + } + + return true; + } +}; + + + +void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +{ + + BT_PROFILE("convexSweepTest"); + /// use the broadphase to accelerate the search for objects, based on their aabb + /// and for each object with ray-aabb overlap, perform an exact ray test + /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical + + + btTransform convexFromTrans,convexToTrans; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; @@ -649,12 +791,21 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT { btVector3 linVel, angVel; btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); + btVector3 zeroLinVel; + zeroLinVel.setValue(0,0,0); btTransform R; R.setIdentity (); R.setRotation (convexFromTrans.getRotation()); - castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); + castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); } +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + + btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration); + + m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax); + +#else /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) int i; @@ -676,9 +827,9 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT collisionObject->getCollisionShape(), collisionObject->getWorldTransform(), resultCallback, - getDispatchInfo().m_allowedCcdPenetration); + allowedCcdPenetration); } } } - +#endif //USE_BRUTEFORCE_RAYBROADPHASE } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 7557691a9a9..87f7137a55b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +Copyright (c) 2003-2006 Erwin Coumans http://bulletphysics.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. @@ -22,39 +22,39 @@ subject to the following restrictions: * * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). * - * There is the Physics Forum for Feedback and bteral Collision Detection and Physics discussions. - * Please visit http://www.continuousphysics.com/Bullet/phpBB2/index.php + * There is the Physics Forum for feedback and general Collision Detection and Physics discussions. + * Please visit http://www.bulletphysics.com * * @section install_sec Installation * * @subsection step1 Step 1: Download - * You can download the Bullet Physics Library from our website: http://www.continuousphysics.com/Bullet/ + * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list * @subsection step2 Step 2: Building * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8. * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version). * - * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using cmake, http://www.cmake.org, or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet. + * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using make, cmake, http://www.cmake.org , or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet. + * So if you are not using MSVC or cmake, you can run ./autogen.sh ./configure to create both Makefile and Jamfile and then run make or jam. * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files. - * So if you are not using MSVC, you can run configure and jam . - * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/pub/jam/ + * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/jam * * @subsection step3 Step 3: Testing demos - * Try to run and experiment with CcdPhysicsDemo executable as a starting point. + * Try to run and experiment with BasicDemo executable as a starting point. * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector Library or Low Level / Snippets like the GJK Closest Point calculation. * The Dependencies can be seen in this documentation under Directories * - * @subsection step4 Step 4: Integrating in your application, Full Rigid Body Simulation - * Check out CcdPhysicsDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform. - * PLEASE NOTE THE CcdPhysicsEnvironment and CcdPhysicsController is obsolete and will be removed. It has been replaced by classes derived frmo btDynamicsWorld and btRididBody + * @subsection step4 Step 4: Integrating in your application, full Rigid Body and Soft Body simulation + * Check out BasicDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform. + * Check out SoftDemo how to use soft body dynamics, using btSoftRigidDynamicsWorld. * @subsection step5 Step 5 : Integrate the Collision Detection Library (without Dynamics and other Extras) * Bullet Collision Detection can also be used without the Dynamics/Extras. - * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo. Also in Extras/test_BulletOde.cpp there is a sample Collision Detection integration with Open Dynamics Engine, ODE, http://www.ode.org + * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo. * @subsection step6 Step 6 : Use Snippets like the GJK Closest Point calculation. * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector. * * @section copyright Copyright - * Copyright (C) 2005-2007 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon - * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky, + * Copyright (C) 2005-2008 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon + * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, John McCutchan, Nathanael Presson, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky, * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren, Marten Svanfeldt. * */ @@ -71,7 +71,7 @@ class btBroadphaseInterface; #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "btCollisionObject.h" -#include "btCollisionDispatcher.h" //for definition of btCollisionObjectArray +#include "btCollisionDispatcher.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" #include "LinearMath/btAlignedObjectArray.h" @@ -107,6 +107,11 @@ public: m_broadphasePairCache = pairCache; } + const btBroadphaseInterface* getBroadphase() const + { + return m_broadphasePairCache; + } + btBroadphaseInterface* getBroadphase() { return m_broadphasePairCache; @@ -128,8 +133,10 @@ public: return m_dispatcher1; } - virtual void updateAabbs(); + void updateSingleAabb(btCollisionObject* colObj); + virtual void updateAabbs(); + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) { m_debugDrawer = debugDrawer; @@ -179,6 +186,8 @@ public: btCollisionObject* m_collisionObject; short int m_collisionFilterGroup; short int m_collisionFilterMask; + //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback + unsigned int m_flags; virtual ~RayResultCallback() { @@ -192,7 +201,9 @@ public: :m_closestHitFraction(btScalar(1.)), m_collisionObject(0), m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter) + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + //@BP Mod + m_flags(0) { } @@ -347,7 +358,7 @@ public: // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. - void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback) const; + void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const; /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 535b61992b0..1dea91a0b0b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -19,19 +19,32 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btDbvt.h" #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btAabbUtil2.h" +#include "btManifoldResult.h" btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) -:btCollisionAlgorithm(ci), +:btActivatingCollisionAlgorithm(ci,body0,body1), m_isSwapped(isSwapped), m_sharedManifold(ci.m_manifold) { m_ownsManifold = false; + btCollisionObject* colObj = m_isSwapped? body1 : body0; + btAssert (colObj->getCollisionShape()->isCompound()); + + btCompoundShape* compoundShape = static_cast(colObj->getCollisionShape()); + m_compoundShapeRevision = compoundShape->getUpdateRevision(); + + preallocateChildAlgorithms(body0,body1); +} + +void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1) +{ btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; - assert (colObj->getCollisionShape()->isCompound()); + btAssert (colObj->getCollisionShape()->isCompound()); btCompoundShape* compoundShape = static_cast(colObj->getCollisionShape()); + int numChildren = compoundShape->getNumChildShapes(); int i; @@ -46,14 +59,13 @@ m_sharedManifold(ci.m_manifold) btCollisionShape* tmpShape = colObj->getCollisionShape(); btCollisionShape* childShape = compoundShape->getChildShape(i); colObj->internalSetTemporaryCollisionShape( childShape ); - m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold); + m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(colObj,otherObj,m_sharedManifold); colObj->internalSetTemporaryCollisionShape( tmpShape ); } } } - -btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() +void btCompoundCollisionAlgorithm::removeChildAlgorithms() { int numChildren = m_childCollisionAlgorithms.size(); int i; @@ -67,6 +79,11 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() } } +btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() +{ + removeChildAlgorithms(); +} + @@ -167,13 +184,50 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; - assert (colObj->getCollisionShape()->isCompound()); + + + btAssert (colObj->getCollisionShape()->isCompound()); btCompoundShape* compoundShape = static_cast(colObj->getCollisionShape()); + ///btCompoundShape might have changed: + ////make sure the internal child collision algorithm caches are still valid + if (compoundShape->getUpdateRevision() != m_compoundShapeRevision) + { + ///clear and update all + removeChildAlgorithms(); + + preallocateChildAlgorithms(body0,body1); + } + + btDbvt* tree = compoundShape->getDynamicAabbTree(); //use a dynamic aabb tree to cull potential child-overlaps btCompoundLeafCallback callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); + ///we need to refresh all contact manifolds + ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep + ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm + { + int i; + btManifoldArray manifoldArray; + for (i=0;igetAllContactManifolds(manifoldArray); + for (int m=0;mgetNumContacts()) + { + resultOut->setPersistentManifold(manifoldArray[m]); + resultOut->refreshContactPoints(); + resultOut->setPersistentManifold(0);//??necessary? + } + } + manifoldArray.clear(); + } + } + } if (tree) { @@ -242,7 +296,7 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* btCollisionObject* colObj = m_isSwapped? body1 : body0; btCollisionObject* otherObj = m_isSwapped? body0 : body1; - assert (colObj->getCollisionShape()->isCompound()); + btAssert (colObj->getCollisionShape()->isCompound()); btCompoundShape* compoundShape = static_cast(colObj->getCollisionShape()); @@ -285,3 +339,4 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* } + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 624a3cf10f5..255e0af668c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef COMPOUND_COLLISION_ALGORITHM_H #define COMPOUND_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" @@ -26,16 +26,23 @@ class btDispatcher; #include "btCollisionCreateFunc.h" #include "LinearMath/btAlignedObjectArray.h" class btDispatcher; +class btCollisionObject; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes -class btCompoundCollisionAlgorithm : public btCollisionAlgorithm +class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm { btAlignedObjectArray m_childCollisionAlgorithms; bool m_isSwapped; class btPersistentManifold* m_sharedManifold; bool m_ownsManifold; + + int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated + + void removeChildAlgorithms(); + void preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1); + public: btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 6d28904cb03..cbc5530732b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) -: btCollisionAlgorithm(ci), +: btActivatingCollisionAlgorithm(ci,body0,body1), m_isSwapped(isSwapped), m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped) { @@ -72,7 +72,7 @@ btConvexTriangleCallback::~btConvexTriangleCallback() void btConvexTriangleCallback::clearCache() { m_dispatcher->clearManifold(m_manifoldPtr); -}; +} @@ -93,7 +93,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i ///debug drawing of the overlapping triangles - if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) + if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) { btVector3 color(255,255,0); btTransform& tr = ob->getWorldTransform(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index 34b9a107be9..984a4c39e8e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef CONVEX_CONCAVE_COLLISION_ALGORITHM_H #define CONVEX_CONCAVE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionShapes/btTriangleCallback.h" @@ -34,8 +34,8 @@ class btConvexTriangleCallback : public btTriangleCallback btVector3 m_aabbMin; btVector3 m_aabbMax ; - btManifoldResult* m_resultOut; + btManifoldResult* m_resultOut; btDispatcher* m_dispatcher; const btDispatcherInfo* m_dispatchInfoPtr; btScalar m_collisionMarginTriangle; @@ -70,7 +70,7 @@ int m_triangleCount; /// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. -class btConvexConcaveCollisionAlgorithm : public btCollisionAlgorithm +class btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm { bool m_isSwapped; @@ -78,6 +78,7 @@ class btConvexConcaveCollisionAlgorithm : public btCollisionAlgorithm btConvexTriangleCallback m_btConvexTriangleCallback; + public: btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index cb830f889be..274c5f5bdc6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -38,7 +38,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" @@ -51,6 +51,8 @@ subject to the following restrictions: btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) { + m_numPerturbationIterations = 0; + m_minimumPointsPerturbationThreshold = 3; m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; } @@ -59,17 +61,22 @@ btConvexConvexAlgorithm::CreateFunc::~CreateFunc() { } -btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) -: btCollisionAlgorithm(ci), -m_gjkPairDetector(0,0,simplexSolver,pdSolver), +btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) +: btActivatingCollisionAlgorithm(ci,body0,body1), +m_simplexSolver(simplexSolver), +m_pdSolver(pdSolver), m_ownManifold (false), m_manifoldPtr(mf), -m_lowLevelOfDetail(false) +m_lowLevelOfDetail(false), +#ifdef USE_SEPDISTANCE_UTIL2 +,m_sepDistance((static_cast(body0->getCollisionShape()))->getAngularMotionDisc(), + (static_cast(body1->getCollisionShape()))->getAngularMotionDisc()), +#endif +m_numPerturbationIterations(numPerturbationIterations), +m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) { (void)body0; (void)body1; - - } @@ -90,8 +97,63 @@ void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) } +struct btPerturbedContactResult : public btManifoldResult +{ + btManifoldResult* m_originalManifoldResult; + btTransform m_transformA; + btTransform m_transformB; + btTransform m_unPerturbedTransform; + bool m_perturbA; + btIDebugDraw* m_debugDrawer; + + + btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer) + :m_originalManifoldResult(originalResult), + m_transformA(transformA), + m_transformB(transformB), + m_perturbA(perturbA), + m_unPerturbedTransform(unPerturbedTransform), + m_debugDrawer(debugDrawer) + { + } + virtual ~ btPerturbedContactResult() + { + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth) + { + btVector3 endPt,startPt; + btScalar newDepth; + btVector3 newNormal; + + if (m_perturbA) + { + btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth; + endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg); + newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); + startPt = endPt+normalOnBInWorld*newDepth; + } else + { + endPt = pointInWorld + normalOnBInWorld*orgDepth; + startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld); + newDepth = (endPt - startPt).dot(normalOnBInWorld); + + } + +//#define DEBUG_CONTACTS 1 +#ifdef DEBUG_CONTACTS + m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0)); + m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0)); + m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1)); +#endif //DEBUG_CONTACTS + + + m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth); + } +}; +extern btScalar gContactBreakingThreshold; // // Convex-Convex collision algorithm @@ -107,39 +169,125 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl } resultOut->setPersistentManifold(m_manifoldPtr); -#ifdef USE_BT_GJKEPA - btConvexShape* shape0(static_cast(body0->getCollisionShape())); - btConvexShape* shape1(static_cast(body1->getCollisionShape())); - const btScalar radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/); - btGjkEpaSolver::sResults results; - if(btGjkEpaSolver::Collide( shape0,body0->getWorldTransform(), - shape1,body1->getWorldTransform(), - radialmargin,results)) - { - dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - } -#else + //comment-out next line to test multi-contact generation + //resultOut->getPersistentManifold()->clearManifold(); + btConvexShape* min0 = static_cast(body0->getCollisionShape()); btConvexShape* min1 = static_cast(body1->getCollisionShape()); + +#ifdef USE_SEPDISTANCE_UTIL2 + m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); + if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) +#endif //USE_SEPDISTANCE_UTIL2 + + { + btGjkPairDetector::ClosestPointInput input; + btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) - m_gjkPairDetector.setMinkowskiA(min0); - m_gjkPairDetector.setMinkowskiB(min1); - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; - input.m_stackAlloc = dispatchInfo.m_stackAllocator; + gjkPairDetector.setMinkowskiA(min0); + gjkPairDetector.setMinkowskiB(min1); -// input.m_maximumDistanceSquared = btScalar(1e30); - +#ifdef USE_SEPDISTANCE_UTIL2 + if (dispatchInfo.m_useConvexConservativeDistanceUtil) + { + input.m_maximumDistanceSquared = 1e30f; + } else +#endif //USE_SEPDISTANCE_UTIL2 + { + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); + input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + } + + input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); + + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold; + + //now perturbe directions to get multiple contact points + btVector3 v0,v1; + btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); + btPlaneSpace1(sepNormalWorldSpace,v0,v1); + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects - m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points + if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) + { + + int i; + + bool perturbeA = true; + const btScalar angleLimit = 0.125f * SIMD_PI; + btScalar perturbeAngle; + btScalar radiusA = min0->getAngularMotionDisc(); + btScalar radiusB = min1->getAngularMotionDisc(); + if (radiusA < radiusB) + { + perturbeAngle = gContactBreakingThreshold /radiusA; + perturbeA = true; + } else + { + perturbeAngle = gContactBreakingThreshold / radiusB; + perturbeA = false; + } + if ( perturbeAngle > angleLimit ) + perturbeAngle = angleLimit; + + btTransform unPerturbedTransform; + if (perturbeA) + { + unPerturbedTransform = input.m_transformA; + } else + { + unPerturbedTransform = input.m_transformB; + } + + for ( i=0;igetWorldTransform().getBasis()); + input.m_transformB = body1->getWorldTransform(); +#ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); +#endif //DEBUG_CONTACTS + } else + { + input.m_transformA = body0->getWorldTransform(); + input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis()); +#ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); #endif + } + + btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + + + } + } + + + +#ifdef USE_SEPDISTANCE_UTIL2 + if (dispatchInfo.m_useConvexConservativeDistanceUtil) + { + m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); + } +#endif //USE_SEPDISTANCE_UTIL2 + + + } if (m_ownManifold) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index a0030e6793d..62dd33eb98d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -16,30 +16,50 @@ subject to the following restrictions: #ifndef CONVEX_CONVEX_ALGORITHM_H #define CONVEX_CONVEX_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil class btConvexPenetrationDepthSolver; -///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations. -class btConvexConvexAlgorithm : public btCollisionAlgorithm +///Enabling USE_SEPDISTANCE_UTIL2 requires 100% reliable distance computation. However, when using large size ratios GJK can be imprecise +///so the distance is not conservative. In that case, enabling this USE_SEPDISTANCE_UTIL2 would result in failing/missing collisions. +///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util +///for certain pairs that have a small size ratio +///#define USE_SEPDISTANCE_UTIL2 1 + +///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects. +///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal. +///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888 +class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm { - btGjkPairDetector m_gjkPairDetector; -public: +#ifdef USE_SEPDISTANCE_UTIL2 + btConvexSeparatingDistanceUtil m_sepDistance; +#endif + btSimplexSolverInterface* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; + bool m_ownManifold; btPersistentManifold* m_manifoldPtr; bool m_lowLevelOfDetail; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + + + ///cache separating vector to speedup collision detection + public: - btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); + btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + virtual ~btConvexConvexAlgorithm(); @@ -65,9 +85,12 @@ public: struct CreateFunc :public btCollisionAlgorithmCreateFunc { + btConvexPenetrationDepthSolver* m_pdSolver; btSimplexSolverInterface* m_simplexSolver; - + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); virtual ~CreateFunc(); @@ -75,7 +98,7 @@ public: virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); - return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver); + return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index 391cf6c7bc4..a7b3b163d66 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -4,8 +4,8 @@ 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, +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. @@ -22,15 +22,17 @@ subject to the following restrictions: //#include -btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold) : btCollisionAlgorithm(ci), m_ownManifold(false), m_manifoldPtr(mf), -m_isSwapped(isSwapped) +m_isSwapped(isSwapped), +m_numPerturbationIterations(numPerturbationIterations), +m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) { btCollisionObject* convexObj = m_isSwapped? col1 : col0; btCollisionObject* planeObj = m_isSwapped? col0 : col1; - + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj)) { m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj); @@ -48,30 +50,28 @@ btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() } } - - -void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { - (void)dispatchInfo; - (void)resultOut; - if (!m_manifoldPtr) - return; - - btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* convexObj = m_isSwapped? body1 : body0; btCollisionObject* planeObj = m_isSwapped? body0: body1; btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); - bool hasCollision = false; + bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); - btTransform planeInConvex; - planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform(); + + btTransform convexWorldTransform = convexObj->getWorldTransform(); btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform(); - + convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform; + //now perturbe the convex-world transform + convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot); + btTransform planeInConvex; + planeInConvex= convexWorldTransform.inverse() * planeObj->getWorldTransform(); + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); @@ -87,6 +87,53 @@ void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0 btVector3 pOnB = vtxInPlaneWorld; resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); } +} + + +void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + if (!m_manifoldPtr) + return; + + btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* planeObj = m_isSwapped? body0: body1; + + btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + + //first perform a collision query with the non-perturbated collision objects + { + btQuaternion rotq(0,0,0,1); + collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut); + } + + if (resultOut->getPersistentManifold()->getNumContacts()getAngularMotionDisc(); + perturbeAngle = gContactBreakingThreshold / radius; + if ( perturbeAngle > angleLimit ) + perturbeAngle = angleLimit; + + btQuaternion perturbeRot(v0,perturbeAngle); + for (int i=0;igetNumContacts()) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h index 7b258554171..368ca71dda0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -4,8 +4,8 @@ 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, +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. @@ -28,18 +28,22 @@ class btPersistentManifold; /// Other features are frame-coherency (persistent data) and collision response. class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm { - bool m_ownManifold; + bool m_ownManifold; btPersistentManifold* m_manifoldPtr; - bool m_isSwapped; - + bool m_isSwapped; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + public: - btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); + btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold); virtual ~btConvexPlaneCollisionAlgorithm(); virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + void collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); virtual void getAllContactManifolds(btManifoldArray& manifoldArray) @@ -52,15 +56,24 @@ public: struct CreateFunc :public btCollisionAlgorithmCreateFunc { + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + + CreateFunc() + : m_numPerturbationIterations(3), + m_minimumPointsPerturbationThreshold(3) + { + } + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); if (!m_swapped) { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false); + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); } else { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true); + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); } } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 1c317080544..3ae25f109c2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -100,11 +100,10 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault int maxSize = sizeof(btConvexConvexAlgorithm); int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); int maxSize3 = sizeof(btCompoundCollisionAlgorithm); - int maxSize4 = sizeof(btEmptyAlgorithm); - + int sl = sizeof(btConvexSeparatingDistanceUtil); + sl = sizeof(btGjkPairDetector); int collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4); if (constructionInfo.m_stackAlloc) { @@ -289,3 +288,10 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg //failed to find an algorithm return m_emptyCreateFunc; } + +void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) +{ + btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc; + convexConvex->m_numPerturbationIterations = numPerturbationIterations; + convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index df48ee95a18..6d8cab726bd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -33,9 +33,9 @@ struct btDefaultCollisionConstructionInfo :m_stackAlloc(0), m_persistentManifoldPool(0), m_collisionAlgorithmPool(0), - m_defaultMaxPersistentManifoldPoolSize(65535), - m_defaultMaxCollisionAlgorithmPoolSize(65535), - m_defaultStackAllocatorSize(5*1024*1024) + m_defaultMaxPersistentManifoldPoolSize(4096), + m_defaultMaxCollisionAlgorithmPoolSize(4096), + m_defaultStackAllocatorSize(0) { } }; @@ -44,7 +44,7 @@ struct btDefaultCollisionConstructionInfo ///btCollisionConfiguration allows to configure Bullet collision detection ///stack allocator, pool memory allocators -///todo: describe the meaning +///@todo: describe the meaning class btDefaultCollisionConfiguration : public btCollisionConfiguration { @@ -111,6 +111,14 @@ public: virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm. + ///By default, this feature is disabled for best performance. + ///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature. + ///@param minimumPointsPerturbationThreshold is the minimum number of points in the contact cache, above which the feature is disabled + ///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first. + ///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points. + ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection. + void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp new file mode 100644 index 00000000000..86141fa6899 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp @@ -0,0 +1,171 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +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 "btGhostObject.h" +#include "btCollisionWorld.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "LinearMath/btAabbUtil2.h" + +btGhostObject::btGhostObject() +{ + m_internalType = CO_GHOST_OBJECT; +} + +btGhostObject::~btGhostObject() +{ + ///btGhostObject should have been removed from the world, so no overlapping objects + btAssert(!m_overlappingObjects.size()); +} + + +void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + ///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index==m_overlappingObjects.size()) + { + //not found + m_overlappingObjects.push_back(otherObject); + } +} + +void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index~btHashedOverlappingPairCache(); + btAlignedFree( m_hashPairCache ); +} + +void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +{ + btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); + btAssert(actualThisProxy); + + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index==m_overlappingObjects.size()) + { + m_overlappingObjects.push_back(otherObject); + m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); + } +} + +void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); + btAssert(actualThisProxy); + + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (indexremoveOverlappingPair(actualThisProxy,otherProxy,dispatcher); + } +} + + +void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +{ + btTransform convexFromTrans,convexToTrans; + convexFromTrans = convexFromWorld; + convexToTrans = convexToWorld; + btVector3 castShapeAabbMin, castShapeAabbMax; + /* Compute AABB that encompasses angular movement */ + { + btVector3 linVel, angVel; + btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); + btTransform R; + R.setIdentity (); + R.setRotation (convexFromTrans.getRotation()); + castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); + } + + /// go over all objects, and if the ray intersects their aabb + cast shape aabb, + // do a ray-shape query using convexCaster (CCD) + int i; + for (i=0;igetBroadphaseHandle())) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + { + btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); + } + } + } + +} + +void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const +{ + btTransform rayFromTrans; + rayFromTrans.setIdentity(); + rayFromTrans.setOrigin(rayFromWorld); + btTransform rayToTrans; + rayToTrans.setIdentity(); + rayToTrans.setOrigin(rayToWorld); + + + int i; + for (i=0;igetBroadphaseHandle())) + { + btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); + } + } +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h new file mode 100644 index 00000000000..95b5750240c --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h @@ -0,0 +1,174 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +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_GHOST_OBJECT_H +#define BT_GHOST_OBJECT_H + + +#include "btCollisionObject.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" +#include "LinearMath/btAlignedAllocator.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "btCollisionWorld.h" + +class btConvexShape; + +class btDispatcher; + +///The btGhostObject can keep track of all objects that are overlapping +///By default, this overlap is based on the AABB +///This is useful for creating a character controller, collision sensors/triggers, explosions etc. +///We plan on adding rayTest and other queries for the btGhostObject +ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject +{ +protected: + + btAlignedObjectArray m_overlappingObjects; + +public: + + btGhostObject(); + + virtual ~btGhostObject(); + + void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; + + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; + + ///this method is mainly for expert/internal use only. + virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + ///this method is mainly for expert/internal use only. + virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + + int getNumOverlappingObjects() const + { + return m_overlappingObjects.size(); + } + + btCollisionObject* getOverlappingObject(int index) + { + return m_overlappingObjects[index]; + } + + const btCollisionObject* getOverlappingObject(int index) const + { + return m_overlappingObjects[index]; + } + + btAlignedObjectArray& getOverlappingPairs() + { + return m_overlappingObjects; + } + + const btAlignedObjectArray getOverlappingPairs() const + { + return m_overlappingObjects; + } + + // + // internal cast + // + + static const btGhostObject* upcast(const btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_GHOST_OBJECT) + return (const btGhostObject*)colObj; + return 0; + } + static btGhostObject* upcast(btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_GHOST_OBJECT) + return (btGhostObject*)colObj; + return 0; + } + +}; + +class btPairCachingGhostObject : public btGhostObject +{ + btHashedOverlappingPairCache* m_hashPairCache; + +public: + + btPairCachingGhostObject(); + + virtual ~btPairCachingGhostObject(); + + ///this method is mainly for expert/internal use only. + virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + + virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + + btHashedOverlappingPairCache* getOverlappingPairCache() + { + return m_hashPairCache; + } + +}; + + + +///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject. +class btGhostPairCallback : public btOverlappingPairCallback +{ + +public: + btGhostPairCallback() + { + } + + virtual ~btGhostPairCallback() + { + + } + + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + { + btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + if (ghost0) + ghost0->addOverlappingObjectInternal(proxy1, proxy0); + if (ghost1) + ghost1->addOverlappingObjectInternal(proxy0, proxy1); + return 0; + } + + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) + { + btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + if (ghost0) + ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0); + if (ghost1) + ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1); + return 0; + } + + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) + { + btAssert(0); + //need to keep track of all ghost objects and call them here + //m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher); + } + + + +}; + +#endif \ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index d5b12598be2..f8dfa5b101f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -55,7 +55,7 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { - assert(m_manifoldPtr); + btAssert(m_manifoldPtr); //order in manifold needs to match if (depth > m_manifoldPtr->getContactBreakingThreshold()) @@ -92,8 +92,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b newPt.m_partId1 = m_partId1; newPt.m_index0 = m_index0; newPt.m_index1 = m_index1; - - ///todo, check this for any side effects + //printf("depth=%f\n",depth); + ///@todo, check this for any side effects if (insertIndex >= 0) { //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 5aac9a46f6a..964b6a04483 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -45,6 +45,8 @@ class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result int m_partId1; int m_index0; int m_index1; + + public: btManifoldResult() @@ -77,6 +79,7 @@ public: m_index1=index1; } + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); SIMD_FORCE_INLINE void refreshContactPoints() diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 9f8e27a4407..0328d0f738f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -24,7 +24,8 @@ subject to the following restrictions: //#include #include "LinearMath/btQuickprof.h" -btSimulationIslandManager::btSimulationIslandManager() +btSimulationIslandManager::btSimulationIslandManager(): +m_splitIslands(true) { } @@ -43,10 +44,10 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl { { - btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); - + for (int i=0;igetPairCache()->getNumOverlappingPairs();i++) { + btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); const btBroadphasePair& collisionPair = pairPtr[i]; btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; @@ -143,11 +144,13 @@ class btPersistentManifoldSortPredicate }; -void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects) +void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) { BT_PROFILE("islandUnionFindAndQuickSort"); + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + m_islandmanifold.resize(0); //we are going to sort the unionfind array, and store the element id in the size @@ -183,7 +186,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio // printf("error in island management\n"); } - assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { if (colObj0->getActivationState()== ACTIVE_TAG) @@ -210,7 +213,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio // printf("error in island management\n"); } - assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { @@ -231,13 +234,14 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio // printf("error in island management\n"); } - assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { if ( colObj0->getActivationState() == ISLAND_SLEEPING) { colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setDeactivationTime(0.f); } } } @@ -248,11 +252,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio int i; int maxNumManifolds = dispatcher->getNumManifolds(); -#define SPLIT_ISLANDS 1 -#ifdef SPLIT_ISLANDS +//#define SPLIT_ISLANDS 1 +//#ifdef SPLIT_ISLANDS -#endif //SPLIT_ISLANDS +//#endif //SPLIT_ISLANDS for (i=0;i(manifold->getBody0()); btCollisionObject* colObj1 = static_cast(manifold->getBody1()); - //todo: check sleeping conditions! + ///@todo: check sleeping conditions! if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) { @@ -276,24 +280,24 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio { colObj0->activate(); } -#ifdef SPLIT_ISLANDS - // //filtering for response - if (dispatcher->needsResponse(colObj0,colObj1)) - m_islandmanifold.push_back(manifold); -#endif //SPLIT_ISLANDS + if(m_splitIslands) + { + //filtering for response + if (dispatcher->needsResponse(colObj0,colObj1)) + m_islandmanifold.push_back(manifold); + } } } } -// -// todo: this is random access, it can be walked 'cache friendly'! -// -void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback) +///@todo: this is random access, it can be walked 'cache friendly'! +void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback) { + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - buildIslands(dispatcher,collisionObjects); + buildIslands(dispatcher,collisionWorld); int endIslandIndex=1; int startIslandIndex; @@ -301,84 +305,86 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, BT_PROFILE("processIslands"); -#ifndef SPLIT_ISLANDS - btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); - - callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); -#else - // Sort manifolds, based on islands - // Sort the vector using predicate and std::sort - //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + if(!m_splitIslands) + { + btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); + int maxNumManifolds = dispatcher->getNumManifolds(); + callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); + } + else + { + // Sort manifolds, based on islands + // Sort the vector using predicate and std::sort + //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); - int numManifolds = int (m_islandmanifold.size()); + int numManifolds = int (m_islandmanifold.size()); - //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) - m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) + m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); - //now process all active islands (sets of manifolds for now) + //now process all active islands (sets of manifolds for now) - int startManifoldIndex = 0; - int endManifoldIndex = 1; + int startManifoldIndex = 0; + int endManifoldIndex = 1; - //int islandId; + //int islandId; - + -// printf("Start Islands\n"); + // printf("Start Islands\n"); - //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - for ( startIslandIndex=0;startIslandIndexisActive()) - islandSleeping = true; - } - + bool islandSleeping = false; + + for (endIslandIndex = startIslandIndex;(endIslandIndexisActive()) + islandSleeping = true; + } + - //find the accompanying contact manifold for this islandId - int numIslandManifolds = 0; - btPersistentManifold** startManifold = 0; + //find the accompanying contact manifold for this islandId + int numIslandManifolds = 0; + btPersistentManifold** startManifold = 0; - if (startManifoldIndexProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); + // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); + } + + if (numIslandManifolds) + { + startManifoldIndex = endManifoldIndex; + } - if (!islandSleeping) - { - callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); -// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); - } - - if (numIslandManifolds) - { - startManifoldIndex = endManifoldIndex; + m_islandBodies.resize(0); } - - m_islandBodies.resize(0); - } -#endif //SPLIT_ISLANDS - + } // else if(!splitIslands) } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index 5f4d54cd803..d059f5d6b0d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -19,7 +19,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btUnionFind.h" #include "btCollisionCreateFunc.h" #include "LinearMath/btAlignedObjectArray.h" - +#include "btCollisionObject.h" class btCollisionObject; class btCollisionWorld; @@ -35,6 +35,7 @@ class btSimulationIslandManager btAlignedObjectArray m_islandmanifold; btAlignedObjectArray m_islandBodies; + bool m_splitIslands; public: btSimulationIslandManager(); @@ -61,9 +62,18 @@ public: virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; }; - void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback); + void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback); + + void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); - void buildIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects); + bool getSplitIslands() + { + return m_splitIslands; + } + void setSplitIslands(bool doSplitIslands) + { + m_splitIslands = doSplitIslands; + } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index 1e4bbce451d..8df876928c1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: //#include btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) -: btCollisionAlgorithm(ci), +: btActivatingCollisionAlgorithm(ci,col0,col1), m_ownManifold(false), m_manifoldPtr(mf), m_isSwapped(isSwapped) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index 158f0ffcc74..47111d1c4af 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef SPHERE_BOX_COLLISION_ALGORITHM_H #define SPHERE_BOX_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; @@ -26,7 +26,7 @@ class btPersistentManifold; /// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. /// Other features are frame-coherency (persistent data) and collision response. -class btSphereBoxCollisionAlgorithm : public btCollisionAlgorithm +class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm { bool m_ownManifold; btPersistentManifold* m_manifoldPtr; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index c2b13f5903f..5c4e78fe518 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1) -: btCollisionAlgorithm(ci), +: btActivatingCollisionAlgorithm(ci,col0,col1), m_ownManifold(false), m_manifoldPtr(mf) { @@ -78,7 +78,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0 } ///point on A (worldspace) - btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; + ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index afdc2ee7fa0..7d07512ca66 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H #define SPHERE_SPHERE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" @@ -26,7 +26,7 @@ class btPersistentManifold; /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. /// Other features are frame-coherency (persistent data) and collision response. /// Also provides the most basic sample for custom/user btCollisionAlgorithm -class btSphereSphereCollisionAlgorithm : public btCollisionAlgorithm +class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm { bool m_ownManifold; btPersistentManifold* m_manifoldPtr; @@ -35,7 +35,7 @@ public: btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btCollisionAlgorithm(ci) {} + : btActivatingCollisionAlgorithm(ci) {} virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index fdbd4abef27..2d5efcf56ba 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped) -: btCollisionAlgorithm(ci), +: btActivatingCollisionAlgorithm(ci,col0,col1), m_ownManifold(false), m_manifoldPtr(mf), m_swapped(swapped) @@ -56,10 +56,10 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); - SphereTriangleDetector detector(sphere,triangle); + SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = btScalar(1e30);//todo: tighter bounds + input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds input.m_transformA = sphereObj->getWorldTransform(); input.m_transformB = triObj->getWorldTransform(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index bbaf228c44c..606c3635ae9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H #define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; @@ -25,7 +25,7 @@ class btPersistentManifold; /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. /// Other features are frame-coherency (persistent data) and collision response. /// Also provides the most basic sample for custom/user btCollisionAlgorithm -class btSphereTriangleCollisionAlgorithm : public btCollisionAlgorithm +class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm { bool m_ownManifold; btPersistentManifold* m_manifoldPtr; @@ -35,7 +35,7 @@ public: btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped); btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btCollisionAlgorithm(ci) {} + : btActivatingCollisionAlgorithm(ci) {} virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index bb01b60fa85..c561df06109 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -14,8 +14,6 @@ subject to the following restrictions: */ #include "btUnionFind.h" -#include - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index 820c8bc858e..e105ecbff18 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -98,8 +98,8 @@ class btUnionFind int find(int x) { - //assert(x < m_N); - //assert(x >= 0); + //btAssert(x < m_N); + //btAssert(x >= 0); while (x != m_elements[x].m_id) { @@ -110,8 +110,8 @@ class btUnionFind m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id; #endif // x = m_elements[x].m_id; - //assert(x < m_N); - //assert(x >= 0); + //btAssert(x < m_N); + //btAssert(x >= 0); } return x; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h index 4d4fbefea37..14502d485dc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -19,7 +19,7 @@ subject to the following restrictions: #include "btPolyhedralConvexShape.h" #include "btCollisionMargin.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "LinearMath/btPoint3.h" +#include "LinearMath/btVector3.h" #include "LinearMath/btMinMax.h" ///The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space. @@ -45,8 +45,6 @@ public: } - virtual int getShapeType() const { return BOX_SHAPE_PROXYTYPE;} - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); @@ -82,8 +80,10 @@ public: } - btBoxShape( const btVector3& boxHalfExtents) + btBoxShape( const btVector3& boxHalfExtents) + : btPolyhedralConvexShape() { + m_shapeType = BOX_SHAPE_PROXYTYPE; btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; }; @@ -117,7 +117,7 @@ public: virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const { //this plane might not be aligned... btVector4 plane ; @@ -161,36 +161,30 @@ public: switch (i) { case 0: - plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); - plane[3] = -halfExtents.x(); + plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x()); break; case 1: - plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); - plane[3] = -halfExtents.x(); + plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x()); break; case 2: - plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); - plane[3] = -halfExtents.y(); + plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y()); break; case 3: - plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); - plane[3] = -halfExtents.y(); + plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y()); break; case 4: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); - plane[3] = -halfExtents.z(); + plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z()); break; case 5: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); - plane[3] = -halfExtents.z(); + plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z()); break; default: - assert(0); + btAssert(0); } } - virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const + virtual void getEdge(int i,btVector3& pa,btVector3& pb) const //virtual void getEdge(int i,Edge& edge) const { int edgeVert0 = 0; @@ -261,7 +255,7 @@ public: - virtual bool isInside(const btPoint3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt,btScalar tolerance) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); @@ -312,7 +306,7 @@ public: penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); break; default: - assert(0); + btAssert(0); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index 819f6f7f97d..68ff5e2d7bb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -26,6 +26,7 @@ m_bvh(0), m_useQuantizedAabbCompression(useQuantizedAabbCompression), m_ownsBvh(false) { + m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; //construct bvh from meshInterface #ifndef DISABLE_BVH @@ -57,6 +58,7 @@ m_bvh(0), m_useQuantizedAabbCompression(useQuantizedAabbCompression), m_ownsBvh(false) { + m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; //construct bvh from meshInterface #ifndef DISABLE_BVH @@ -141,10 +143,19 @@ void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const for (int j=2;j>=0;j--) { int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - - btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + + if (type == PHY_FLOAT) + { + float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + } + else + { + double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ()); + } } /* Perform ray vs. triangle collision here */ @@ -202,9 +213,18 @@ void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, co { int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + if (type == PHY_FLOAT) + { + float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + } + else + { + double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ()); + } } /* Perform ray vs. triangle collision here */ @@ -279,12 +299,24 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co #ifdef DEBUG_TRIANGLE_MESH printf("%d ,",graphicsindex); #endif //DEBUG_TRIANGLE_MESH - btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); + if (type == PHY_FLOAT) + { + float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3( + graphicsbase[0]*meshScaling.getX(), + graphicsbase[1]*meshScaling.getY(), + graphicsbase[2]*meshScaling.getZ()); + } + else + { + double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3( + btScalar(graphicsbase[0])*meshScaling.getX(), + btScalar(graphicsbase[1])*meshScaling.getY(), + btScalar(graphicsbase[2])*meshScaling.getZ()); + } #ifdef DEBUG_TRIANGLE_MESH printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z()); #endif //DEBUG_TRIANGLE_MESH diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 6e587340dc1..cb2c326574e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -37,7 +37,7 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btBvhTriangleMeshShape() :btTriangleMeshShape(0),m_bvh(0),m_ownsBvh(false) {}; + btBvhTriangleMeshShape() : btTriangleMeshShape(0),m_bvh(0),m_ownsBvh(false) {m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;}; btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true); ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb @@ -50,10 +50,7 @@ public: return m_ownsBvh; } - virtual int getShapeType() const - { - return TRIANGLE_MESH_SHAPE_PROXYTYPE; - } + void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget); void performConvexcast (btTriangleCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index 26ba276b900..60a96a542f9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -19,8 +19,9 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" -btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) +btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () { + m_shapeType = CAPSULE_SHAPE_PROXYTYPE; m_upAxis = 1; m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h index d4b046d40c9..828c1b3a565 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -30,7 +30,7 @@ protected: protected: ///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses. - btCapsuleShape() {}; + btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;}; public: btCapsuleShape(btScalar radius,btScalar height); @@ -43,15 +43,13 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual int getShapeType() const { return CAPSULE_SHAPE_PROXYTYPE; } - virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { btVector3 halfExtents(getRadius(),getRadius(),getRadius()); halfExtents[m_upAxis] = getRadius() + getHalfHeight(); halfExtents += btVector3(getMargin(),getMargin(),getMargin()); btMatrix3x3 abs_b = t.getBasis().absolute(); - btPoint3 center = t.getOrigin(); + btVector3 center = t.getOrigin(); btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); aabbMin = center - extent; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp index 39e621aa946..d242cba1b72 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -16,6 +16,9 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCollisionShape.h" +btScalar gContactThresholdFactor=btScalar(0.02); + + /* Make sure this dummy function never changes so that it can be used by probes that are checking whether the @@ -42,8 +45,13 @@ void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) con center = (aabbMin+aabbMax)*btScalar(0.5); } +btScalar btCollisionShape::getContactBreakingThreshold() const +{ + return getAngularMotionDisc() * gContactThresholdFactor; +} btScalar btCollisionShape::getAngularMotionDisc() const { + ///@todo cache this value, to improve performance btVector3 center; btScalar disc; getBoundingSphere(center,disc); @@ -65,7 +73,7 @@ void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const b // add linear motion btVector3 linMotion = linvel*timeStep; - //todo: simd would have a vector max/min operation, instead of per-element access + ///@todo: simd would have a vector max/min operation, instead of per-element access if (linMotion.x() > btScalar(0.)) temporalAabbMaxx += linMotion.x(); else diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index 3a352b90777..1f4b9bec647 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -19,20 +19,21 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btPoint3.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types ///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects. class btCollisionShape { - +protected: + int m_shapeType; void* m_userPointer; public: - btCollisionShape() : m_userPointer(0) + btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0) { } + virtual ~btCollisionShape() { } @@ -45,6 +46,8 @@ public: ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. virtual btScalar getAngularMotionDisc() const; + virtual btScalar getContactBreakingThreshold() const; + ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative @@ -76,7 +79,7 @@ public: return btBroadphaseProxy::isInfinite(getShapeType()); } - virtual int getShapeType() const=0; + virtual void setLocalScaling(const btVector3& scaling) =0; virtual const btVector3& getLocalScaling() const =0; virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0; @@ -87,13 +90,13 @@ public: #endif //__SPU__ - + int getShapeType() const { return m_shapeType; } virtual void setMargin(btScalar margin) = 0; virtual btScalar getMargin() const = 0; ///optional user data pointer - void setUserPointer(void* userPtr) + void setUserPointer(void* userPtr) { m_userPointer = userPtr; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 997361a5069..9c2b04d18fa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -17,16 +17,22 @@ subject to the following restrictions: #include "btCollisionShape.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" -btCompoundShape::btCompoundShape() -:m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)), +btCompoundShape::btCompoundShape(bool enableDynamicAabbTree) +: m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)), m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)), m_collisionMargin(btScalar(0.)), m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), -m_dynamicAabbTree(0) +m_dynamicAabbTree(0), +m_updateRevision(1) { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); + m_shapeType = COMPOUND_SHAPE_PROXYTYPE; + + if (enableDynamicAabbTree) + { + void* mem = btAlignedAlloc(sizeof(btDbvt),16); + m_dynamicAabbTree = new(mem) btDbvt(); + btAssert(mem==m_dynamicAabbTree); + } } @@ -41,6 +47,7 @@ btCompoundShape::~btCompoundShape() void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) { + m_updateRevision++; //m_childTransforms.push_back(localTransform); //m_childShapes.push_back(shape); btCompoundShapeChild child; @@ -49,6 +56,7 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio child.m_childShapeType = shape->getShapeType(); child.m_childMargin = shape->getMargin(); + //extend the local aabbMin/aabbMax btVector3 localAabbMin,localAabbMax; shape->getAabb(localTransform,localAabbMin,localAabbMax); @@ -72,10 +80,29 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio } m_children.push_back(child); + +} + +void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform) +{ + m_children[childIndex].m_transform = newChildTransform; + + if (m_dynamicAabbTree) + { + ///update the dynamic aabb tree + btVector3 localAabbMin,localAabbMax; + m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax); + ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + int index = m_children.size()-1; + m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds); + } + + recalculateLocalAabb(); } void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) { + m_updateRevision++; btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size()); if (m_dynamicAabbTree) { @@ -86,8 +113,11 @@ void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) } + + void btCompoundShape::removeChildShape(btCollisionShape* shape) { + m_updateRevision++; // Find the children containing the shape specified, and remove those children. //note: there might be multiple children using the same shape! for(int i = m_children.size()-1; i >= 0 ; i--) @@ -97,6 +127,8 @@ void btCompoundShape::removeChildShape(btCollisionShape* shape) m_children.swap(i,m_children.size()-1); m_children.pop_back(); //remove it from the m_dynamicAabbTree too + //@todo: this leads to problems due to caching in the btCompoundCollisionAlgorithm + //so effectively, removeChildShape is broken at the moment //m_dynamicAabbTree->remove(m_aabbProxies[i]); //m_aabbProxies.swap(i,m_children.size()-1); //m_aabbProxies.pop_back(); @@ -112,6 +144,7 @@ void btCompoundShape::recalculateLocalAabb() { // Recalculate the local aabb // Brute force, it iterates over all the shapes left. + m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); @@ -134,19 +167,27 @@ void btCompoundShape::recalculateLocalAabb() void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const { btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); - localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); + + //avoid an illegal AABB when there are no children + if (!m_children.size()) + { + localHalfExtents.setValue(0,0,0); + localCenter.setValue(0,0,0); + } + localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); + btMatrix3x3 abs_b = trans.getBasis().absolute(); - btPoint3 center = trans(localCenter); + btVector3 center = trans(localCenter); btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), abs_b[1].dot(localHalfExtents), abs_b[2].dot(localHalfExtents)); aabbMin = center-extent; aabbMax = center+extent; - + } void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const @@ -178,7 +219,9 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf btScalar totalMass = 0; btVector3 center(0, 0, 0); - for (int k = 0; k < n; k++) + int k; + + for (k = 0; k < n; k++) { center += m_children[k].m_transform.getOrigin() * masses[k]; totalMass += masses[k]; @@ -187,7 +230,7 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf principal.setOrigin(center); btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0); - for (int k = 0; k < n; k++) + for ( k = 0; k < n; k++) { btVector3 i; m_children[k].m_childShape->calculateLocalInertia(masses[k], i); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 3624749f768..434860c8633 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -46,23 +46,26 @@ SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompou c1.m_childMargin == c2.m_childMargin ); } -/// btCompoundShape allows to store multiple other btCollisionShapes +/// The btCompoundShape allows to store multiple other btCollisionShapes /// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape. +/// It has an (optional) dynamic aabb tree to accelerate early rejection tests. +/// @todo: This aabb tree can also be use to speed up ray tests on btCompoundShape, see http://code.google.com/p/bullet/issues/detail?id=25 +/// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape) ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape { - //btAlignedObjectArray m_childTransforms; - //btAlignedObjectArray m_childShapes; btAlignedObjectArray m_children; btVector3 m_localAabbMin; btVector3 m_localAabbMax; - //btOptimizedBvh* m_aabbTree; btDbvt* m_dynamicAabbTree; + ///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated + int m_updateRevision; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btCompoundShape(); + btCompoundShape(bool enableDynamicAabbTree = true); virtual ~btCompoundShape(); @@ -88,15 +91,18 @@ public: return m_children[index].m_childShape; } - btTransform getChildTransform(int index) + btTransform& getChildTransform(int index) { return m_children[index].m_transform; } - const btTransform getChildTransform(int index) const + const btTransform& getChildTransform(int index) const { return m_children[index].m_transform; } + ///set a new transform for a child, and update internal data structures (local aabb and dynamic tree) + void updateChildTransform(int childIndex, const btTransform& newChildTransform); + btCompoundShapeChild* getChildList() { @@ -121,8 +127,6 @@ public: virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - virtual int getShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;} - virtual void setMargin(btScalar margin) { m_collisionMargin = margin; @@ -151,6 +155,10 @@ public: ///of the collision object by the principal transform. void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const; + int getUpdateRevision() const + { + return m_updateRevision; + } private: btScalar m_collisionMargin; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h index f3f57206ab7..30065d55a12 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -20,6 +20,16 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "btTriangleCallback.h" +/// PHY_ScalarType enumerates possible scalar types. +/// See the btStridingMeshInterface or btHeightfieldTerrainShape for its use +typedef enum PHY_ScalarType { + PHY_FLOAT, + PHY_DOUBLE, + PHY_INTEGER, + PHY_SHORT, + PHY_FIXEDPOINT88, + PHY_UCHAR +} PHY_ScalarType; ///The btConcaveShape class provides an interface for non-moving (static) concave shapes. ///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp index 207b3024bc3..d887be61ada 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -14,14 +14,14 @@ subject to the following restrictions: */ #include "btConeShape.h" -#include "LinearMath/btPoint3.h" -btConeShape::btConeShape (btScalar radius,btScalar height): +btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (), m_radius (radius), m_height(height) { + m_shapeType = CONE_SHAPE_PROXYTYPE; setConeUpIndex(1); btVector3 halfExtents; m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height)); @@ -60,7 +60,7 @@ void btConeShape::setConeUpIndex(int upIndex) m_coneIndices[2] = 1; break; default: - assert(0); + btAssert(0); }; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index 685369742e4..1869893f37a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -69,9 +69,6 @@ public: } - - virtual int getShapeType() const { return CONE_SHAPE_PROXYTYPE; } - virtual const char* getName()const { return "Cone"; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 2596858bc3a..2b067367767 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -19,16 +19,17 @@ subject to the following restrictions: -btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) +btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexShape () { - m_points.resize(numPoints); + m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE; + m_unscaledPoints.resize(numPoints); unsigned char* pointsBaseAddress = (unsigned char*)points; for (int i=0;i maxDot) @@ -91,9 +92,9 @@ void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const supportVerticesOut[i][3] = btScalar(-1e30); } } - for (int i=0;i m_points; + btAlignedObjectArray m_unscaledPoints; public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -33,23 +33,38 @@ public: ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive btScalar (x,y,z), the striding defines the number of bytes between each point, in memory. ///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint. ///btConvexHullShape make an internal copy of the points. - btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btPoint3)); + btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btVector3)); - void addPoint(const btPoint3& point); + void addPoint(const btVector3& point); - btPoint3* getPoints() + + btVector3* getUnscaledPoints() + { + return &m_unscaledPoints[0]; + } + + const btVector3* getUnscaledPoints() const + { + return &m_unscaledPoints[0]; + } + + ///getPoints is obsolete, please use getUnscaledPoints + const btVector3* getPoints() const { - return &m_points[0]; + return getUnscaledPoints(); } - const btPoint3* getPoints() const + + + + SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const { - return &m_points[0]; + return m_unscaledPoints[i] * m_localScaling; } - int getNumPoints() const + SIMD_FORCE_INLINE int getNumPoints() const { - return m_points.size(); + return m_unscaledPoints.size(); } virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; @@ -57,7 +72,6 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual int getShapeType()const { return CONVEX_HULL_SHAPE_PROXYTYPE; } //debugging virtual const char* getName()const {return "Convex";} @@ -65,11 +79,11 @@ public: virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const; - virtual void getVertex(int i,btPoint3& vtx) const; + virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; + virtual void getVertex(int i,btVector3& vtx) const; virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const; - virtual bool isInside(const btPoint3& pt,btScalar tolerance) const; + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; + virtual bool isInside(const btVector3& pt,btScalar tolerance) const; ///in case we receive negative scaling virtual void setLocalScaling(const btVector3& scaling); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp index fb81c8a5bde..bfb741310e1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp @@ -17,6 +17,7 @@ subject to the following restrictions: #include "btConvexInternalShape.h" + btConvexInternalShape::btConvexInternalShape() : m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), m_collisionMargin(CONVEX_DISTANCE_MARGIN) @@ -48,7 +49,8 @@ void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAa tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); minAabb[i] = tmp[i]-margin; } -}; +} + btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const @@ -70,6 +72,7 @@ btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)c return supVertex; #else + btAssert(0); return btVector3(0,0,0); #endif //__SPU__ diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h index 9388ccf31f2..bab720d7b6d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -19,23 +19,18 @@ class btConvexInternalShape : public btConvexShape btScalar m_padding; + btConvexInternalShape(); + public: - btConvexInternalShape(); + virtual ~btConvexInternalShape() { } - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; -#ifndef __SPU__ - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0; - - //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; -#endif //#ifndef __SPU__ const btVector3& getImplicitShapeDimensions() const { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp new file mode 100644 index 00000000000..4ab5d692945 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp @@ -0,0 +1,156 @@ +/* +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 "btConvexPointCloudShape.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +#include "LinearMath/btQuaternion.h" + +void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; + recalcLocalAabb(); +} + +#ifndef __SPU__ +btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +{ + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar newDot,maxDot = btScalar(-1e30); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + + + for (int i=0;i maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return supVec; +} + +void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + btScalar newDot; + //use 'w' component of supportVerticesOut? + { + for (int i=0;i supportVerticesOut[j][3]) + { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j] = vtx; + supportVerticesOut[j][3] = newDot; + } + } + } + + + +} + + + +btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const +{ + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + + if ( getMargin()!=btScalar(0.) ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); + } + vecnorm.normalize(); + supVertex+= getMargin() * vecnorm; + } + return supVertex; +} + + +#endif + + + + + + +//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection +//Please note that you can debug-draw btConvexHullShape with the Raytracer Demo +int btConvexPointCloudShape::getNumVertices() const +{ + return m_numPoints; +} + +int btConvexPointCloudShape::getNumEdges() const +{ + return 0; +} + +void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const +{ + btAssert (0); +} + +void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const +{ + vtx = m_unscaledPoints[i]*m_localScaling; +} + +int btConvexPointCloudShape::getNumPlanes() const +{ + return 0; +} + +void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const +{ + + btAssert(0); +} + +//not yet +bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const +{ + btAssert(0); + return false; +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h new file mode 100644 index 00000000000..7e1c13b29c1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h @@ -0,0 +1,96 @@ +/* +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_CONVEX_POINT_CLOUD_SHAPE_H +#define BT_CONVEX_POINT_CLOUD_SHAPE_H + +#include "btPolyhedralConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "LinearMath/btAlignedObjectArray.h" + +///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices. +ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexShape +{ + btVector3* m_unscaledPoints; + int m_numPoints; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true) + { + m_localScaling = localScaling; + m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; + m_unscaledPoints = points; + m_numPoints = numPoints; + + if (computeAabb) + recalcLocalAabb(); + } + + void setPoints (btVector3* points, int numPoints, bool computeAabb = true) + { + m_unscaledPoints = points; + m_numPoints = numPoints; + + if (computeAabb) + recalcLocalAabb(); + } + + SIMD_FORCE_INLINE btVector3* getUnscaledPoints() + { + return m_unscaledPoints; + } + + SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const + { + return m_unscaledPoints; + } + + SIMD_FORCE_INLINE int getNumPoints() const + { + return m_numPoints; + } + + SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const + { + return m_unscaledPoints[index] * m_localScaling; + } + +#ifndef __SPU__ + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; +#endif + + + //debugging + virtual const char* getName()const {return "ConvexPointCloud";} + + virtual int getNumVertices() const; + virtual int getNumEdges() const; + virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; + virtual void getVertex(int i,btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; + virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + + ///in case we receive negative scaling + virtual void setLocalScaling(const btVector3& scaling); +}; + + +#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 7afcccf8b03..7e67696f4c7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -14,5 +14,366 @@ subject to the following restrictions: */ #include "btConvexShape.h" +#include "btTriangleShape.h" +#include "btSphereShape.h" +#include "btCylinderShape.h" +#include "btCapsuleShape.h" +#include "btConvexHullShape.h" +#include "btConvexPointCloudShape.h" +btConvexShape::btConvexShape () +{ +} +btConvexShape::~btConvexShape() +{ + +} + + + +static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints, const btVector3& localScaling) +{ + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar newDot,maxDot = btScalar(-1e30); + + btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + + + for (int i=0;i maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); +} + +btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const +{ + switch (m_shapeType) + { + case SPHERE_SHAPE_PROXYTYPE: + { + return btVector3(0,0,0); + } + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* convexShape = (btBoxShape*)this; + const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); + + return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), + btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), + btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); + } + case TRIANGLE_SHAPE_PROXYTYPE: + { + btTriangleShape* triangleShape = (btTriangleShape*)this; + btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); + btVector3* vertices = &triangleShape->m_vertices1[0]; + btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2])); + btVector3 sup = vertices[dots.maxAxis()]; + return btVector3(sup.getX(),sup.getY(),sup.getZ()); + } + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShape* cylShape = (btCylinderShape*)this; + //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) + + btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); + btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); + int cylinderUpAxis = cylShape->getUpAxis(); + int XX(1),YY(0),ZZ(2); + + switch (cylinderUpAxis) + { + case 0: + { + XX = 1; + YY = 0; + ZZ = 2; + } + break; + case 1: + { + XX = 0; + YY = 1; + ZZ = 2; + } + break; + case 2: + { + XX = 0; + YY = 2; + ZZ = 1; + + } + break; + default: + btAssert(0); + break; + }; + + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; + + btVector3 tmp; + btScalar d ; + + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) + { + d = radius / s; + tmp[XX] = v[XX] * d; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = v[ZZ] * d; + return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); + } else { + tmp[XX] = radius; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = btScalar(0.0); + return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); + } + } + case CAPSULE_SHAPE_PROXYTYPE: + { + btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); + + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + btScalar halfHeight = capsuleShape->getHalfHeight(); + int capsuleUpAxis = capsuleShape->getUpAxis(); + + btScalar radius = capsuleShape->getRadius(); + btVector3 supVec(0,0,0); + + btScalar maxDot(btScalar(-1e30)); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + btVector3 vtx; + btScalar newDot; + { + btVector3 pos(0,0,0); + pos[capsuleUpAxis] = halfHeight; + + //vtx = pos +vec*(radius); + vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV(); + newDot = vec.dot(vtx); + + + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + { + btVector3 pos(0,0,0); + pos[capsuleUpAxis] = -halfHeight; + + //vtx = pos +vec*(radius); + vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); + } + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + { + btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; + btVector3* points = convexPointCloudShape->getUnscaledPoints (); + int numPoints = convexPointCloudShape->getNumPoints (); + return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV()); + } + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btConvexHullShape* convexHullShape = (btConvexHullShape*)this; + btVector3* points = convexHullShape->getUnscaledPoints(); + int numPoints = convexHullShape->getNumPoints (); + return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV()); + } + default: +#ifndef __SPU__ + return this->localGetSupportingVertexWithoutMargin (localDir); +#else + btAssert (0); +#endif + } + + // should never reach here + btAssert (0); + return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); +} + +btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const +{ + btVector3 localDirNorm = localDir; + if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); + } + localDirNorm.normalize (); + + return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm; +} + +/* TODO: This should be bumped up to btCollisionShape () */ +btScalar btConvexShape::getMarginNonVirtual () const +{ + switch (m_shapeType) + { + case SPHERE_SHAPE_PROXYTYPE: + { + btSphereShape* sphereShape = (btSphereShape*)this; + return sphereShape->getRadius (); + } + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* convexShape = (btBoxShape*)this; + return convexShape->getMarginNV (); + } + case TRIANGLE_SHAPE_PROXYTYPE: + { + btTriangleShape* triangleShape = (btTriangleShape*)this; + return triangleShape->getMarginNV (); + } + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShape* cylShape = (btCylinderShape*)this; + return cylShape->getMarginNV(); + } + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + return capsuleShape->getMarginNV(); + } + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + /* fall through */ + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; + return convexHullShape->getMarginNV(); + } + default: +#ifndef __SPU__ + return this->getMargin (); +#else + btAssert (0); +#endif + } + + // should never reach here + btAssert (0); + return btScalar(0.0f); +} + +void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const +{ + switch (m_shapeType) + { + case SPHERE_SHAPE_PROXYTYPE: + { + btSphereShape* sphereShape = (btSphereShape*)this; + btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); + btScalar margin = radius + sphereShape->getMarginNonVirtual(); + const btVector3& center = t.getOrigin(); + btVector3 extent(margin,margin,margin); + aabbMin = center - extent; + aabbMax = center + extent; + } + break; + case CYLINDER_SHAPE_PROXYTYPE: + /* fall through */ + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* convexShape = (btBoxShape*)this; + btScalar margin=convexShape->getMarginNonVirtual(); + btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); + halfExtents += btVector3(margin,margin,margin); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); + + aabbMin = center - extent; + aabbMax = center + extent; + break; + } + case TRIANGLE_SHAPE_PROXYTYPE: + { + btTriangleShape* triangleShape = (btTriangleShape*)this; + btScalar margin = triangleShape->getMarginNonVirtual(); + for (int i=0;i<3;i++) + { + btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + vec[i] = btScalar(1.); + + btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()); + + btVector3 tmp = t(sv); + aabbMax[i] = tmp[i]+margin; + vec[i] = btScalar(-1.); + tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis())); + aabbMin[i] = tmp[i]-margin; + } + } + break; + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius()); + int m_upAxis = capsuleShape->getUpAxis(); + halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); + halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual()); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); + aabbMin = center - extent; + aabbMax = center + extent; + } + break; + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; + btScalar margin = convexHullShape->getMarginNonVirtual(); + convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin); + } + break; + default: +#ifndef __SPU__ + this->getAabb (t, aabbMin, aabbMax); +#else + btAssert (0); +#endif + break; + } + + // should never reach here + btAssert (0); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h index e4eb7f6dbf6..0cc7b349521 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -24,8 +24,6 @@ subject to the following restrictions: #include "btCollisionMargin.h" #include "LinearMath/btAlignedAllocator.h" -//todo: get rid of this btConvexCastResult thing! -struct btConvexCastResult; #define MAX_PREFERRED_PENETRATION_DIRECTIONS 10 /// The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape, btConvexHullShape etc. @@ -38,20 +36,25 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); - virtual ~btConvexShape() - { + btConvexShape (); - } + virtual ~btConvexShape(); + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const = 0; + + //////// + #ifndef __SPU__ + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const=0; + #endif //#ifndef __SPU__ + + btVector3 localGetSupportVertexWithoutMarginNonVirtual (const btVector3& vec) const; + btVector3 localGetSupportVertexNonVirtual (const btVector3& vec) const; + btScalar getMarginNonVirtual () const; + void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const =0; -#ifndef __SPU__ - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0; //notice that the vectors should be unit length virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; -#endif //#ifndef __SPU__ - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; @@ -69,6 +72,9 @@ public: virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0; + + + }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index 02a293e82ba..4bd986bb7f7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -20,8 +20,9 @@ subject to the following restrictions: btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) -:m_stridingMesh(meshInterface) +: btPolyhedralConvexShape(), m_stridingMesh(meshInterface) { + m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; if ( calcAabb ) recalcLocalAabb(); } @@ -107,7 +108,7 @@ void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargi } } - //todo: could do the batch inside the callback! + ///@todo: could do the batch inside the callback! for (int j=0;j 1 && "bad width"); + btAssert(heightStickLength > 1 && "bad length"); + btAssert(heightfieldData && "null heightfield data"); + // btAssert(heightScale) -- do we care? Trust caller here + btAssert(minHeight <= maxHeight && "bad min/max height"); + btAssert(upAxis >= 0 && upAxis < 3 && + "bad upAxis--should be in range [0,2]"); + btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT && + "Bad height data type enum"); + + // initialize member variables + m_shapeType = TERRAIN_SHAPE_PROXYTYPE; + m_heightStickWidth = heightStickWidth; + m_heightStickLength = heightStickLength; + m_minHeight = minHeight; + m_maxHeight = maxHeight; + m_width = (btScalar) (heightStickWidth - 1); + m_length = (btScalar) (heightStickLength - 1); + m_heightScale = heightScale; + m_heightfieldDataUnknown = heightfieldData; + m_heightDataType = hdt; + m_flipQuadEdges = flipQuadEdges; + m_useDiamondSubdivision = false; + m_upAxis = upAxis; + m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); + + // determine min/max axis-aligned bounding box (aabb) values switch (m_upAxis) { case 0: { - halfExtents.setValue( - btScalar(m_maxHeight), - btScalar(m_width), //?? don't know if this should change - btScalar(m_length)); + m_localAabbMin.setValue(m_minHeight, 0, 0); + m_localAabbMax.setValue(m_maxHeight, m_width, m_length); break; } case 1: { - halfExtents.setValue( - btScalar(m_width), - btScalar(m_maxHeight), - btScalar(m_length)); + m_localAabbMin.setValue(0, m_minHeight, 0); + m_localAabbMax.setValue(m_width, m_maxHeight, m_length); break; }; case 2: { - halfExtents.setValue( - btScalar(m_width), - btScalar(m_length), - btScalar(m_maxHeight) - ); + m_localAabbMin.setValue(0, 0, m_minHeight); + m_localAabbMax.setValue(m_width, m_length, m_maxHeight); break; } default: { //need to get valid m_upAxis - btAssert(0); + btAssert(0 && "Bad m_upAxis"); } } - halfExtents*= btScalar(0.5); - - m_localAabbMin = -halfExtents - clampValue; - m_localAabbMax = halfExtents + clampValue; - btVector3 aabbSize = m_localAabbMax - m_localAabbMin; - + // remember origin (defined as exact middle of aabb) + m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); } + btHeightfieldTerrainShape::~btHeightfieldTerrainShape() { } @@ -92,57 +128,80 @@ btHeightfieldTerrainShape::~btHeightfieldTerrainShape() void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); - halfExtents += btVector3(getMargin(),getMargin(),getMargin()); + + btVector3 localOrigin(0, 0, 0); + localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); + localOrigin *= m_localScaling; btMatrix3x3 abs_b = t.getBasis().absolute(); - btPoint3 center = t.getOrigin(); + btVector3 center = t.getOrigin(); btVector3 extent = btVector3(abs_b[0].dot(halfExtents), abs_b[1].dot(halfExtents), abs_b[2].dot(halfExtents)); - + extent += btVector3(getMargin(),getMargin(),getMargin()); aabbMin = center - extent; aabbMax = center + extent; - - } -btScalar btHeightfieldTerrainShape::getHeightFieldValue(int x,int y) const + +/// This returns the "raw" (user's initial) height, not the actual height. +/// The actual height needs to be adjusted to be relative to the center +/// of the heightfield's AABB. +btScalar +btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const { btScalar val = 0.f; - if (m_useFloatData) + switch (m_heightDataType) { - val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; - } else - { - //assume unsigned short int - unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; - val = heightFieldValue* (m_maxHeight/btScalar(65535)); + case PHY_FLOAT: + { + val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; + break; + } + + case PHY_UCHAR: + { + unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; + val = heightFieldValue * m_heightScale; + break; + } + + case PHY_SHORT: + { + short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; + val = hfValue * m_heightScale; + break; + } + + default: + { + btAssert(!"Bad m_heightDataType"); + } } + return val; } - +/// this returns the vertex in bullet-local coordinates void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const { - btAssert(x>=0); btAssert(y>=0); btAssert(xstartX) startX = quantizedAabbMin[1]; if (quantizedAabbMax[1]startX) startX = quantizedAabbMin[0]; if (quantizedAabbMax[0]startX) startX = quantizedAabbMin[0]; if (quantizedAabbMax[0] aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; - return overlap; - } + + + /// protected initialization + /** + Handles the work of constructors so that public constructors can be + backwards-compatible without a lot of copy/paste. + */ + void initialize(int heightStickWidth, int heightStickLength, + void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType heightDataType, bool flipQuadEdges); public: - btHeightfieldTerrainShape(int heightStickWidth,int heightStickHeight,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); + /// preferred constructor + /** + This constructor supports a range of heightfield + data types, and allows for a non-zero minimum height value. + heightScale is needed for any integer-based heightfield data types. + */ + btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength, + void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, + int upAxis, PHY_ScalarType heightDataType, + bool flipQuadEdges); + + /// legacy constructor + /** + The legacy constructor assumes the heightfield has a minimum height + of zero. Only unsigned char or floats are supported. For legacy + compatibility reasons, heightScale is calculated as maxHeight / 65535 + (and is only used when useFloatData = false). + */ + btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); virtual ~btHeightfieldTerrainShape(); void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} - virtual int getShapeType() const - { - return TERRAIN_SHAPE_PROXYTYPE; - } virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 8f893ac98aa..b107b0bea6e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -17,23 +17,25 @@ subject to the following restrictions: btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB) -:m_shapeA(shapeA), +: btConvexInternalShape (), +m_shapeA(shapeA), m_shapeB(shapeB) { + m_shapeType = MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE; m_transA.setIdentity(); m_transB.setIdentity(); } btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(-vec*m_transA.getBasis())); - btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis())); + btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis())); + btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec*m_transB.getBasis())); return supVertexA - supVertexB; } void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const { - //todo: could make recursive use of batching. probably this shape is not used frequently. + ///@todo: could make recursive use of batching. probably this shape is not used frequently. for (int i=0;iprocessAllTriangles(&triBuf,aabbMin, aabbMax); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index b962829041a..5f292532861 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -82,7 +82,8 @@ bool btTriangleIndexVertexArray::hasPremadeAabb() const return (m_hasAabb == 1); } -void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) + +void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const { m_aabbMin = aabbMin; m_aabbMax = aabbMax; @@ -95,3 +96,4 @@ void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* a *aabbMax = m_aabbMax; } + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index e546675f802..eb79ff5e47d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -52,9 +52,9 @@ ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshIn protected: IndexedMeshArray m_indexedMeshes; int m_pad[2]; - int m_hasAabb; // using int instead of bool to maintain alignment - btVector3 m_aabbMin; - btVector3 m_aabbMax; + mutable int m_hasAabb; // using int instead of bool to maintain alignment + mutable btVector3 m_aabbMin; + mutable btVector3 m_aabbMax; public: @@ -106,7 +106,7 @@ public: virtual void preallocateIndices(int numindices){(void) numindices;} virtual bool hasPremadeAabb() const; - virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ); + virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const; virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp index 492854ff646..abefa71250a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp @@ -1,3 +1,4 @@ + /* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp index 29d26316316..5dcfa51c280 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -35,13 +35,13 @@ m_weldingThreshold(0.0) if (m_use32bitIndices) { m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3; - m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0]; + m_indexedMeshes[0].m_triangleIndexBase = 0; m_indexedMeshes[0].m_indexType = PHY_INTEGER; m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int); } else { m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3; - m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0]; + m_indexedMeshes[0].m_triangleIndexBase = 0; m_indexedMeshes[0].m_indexType = PHY_SHORT; m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int); } @@ -49,12 +49,12 @@ m_weldingThreshold(0.0) if (m_use4componentVertices) { m_indexedMeshes[0].m_numVertices = m_4componentVertices.size(); - m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0]; + m_indexedMeshes[0].m_vertexBase = 0; m_indexedMeshes[0].m_vertexStride = sizeof(btVector3); } else { m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3; - m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0]; + m_indexedMeshes[0].m_vertexBase = 0; m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar); } @@ -74,17 +74,21 @@ void btTriangleMesh::addIndex(int index) } } -int btTriangleMesh::findOrAddVertex(const btVector3& vertex) + +int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices) { //return index of new/existing vertex - //todo: could use acceleration structure for this + ///@todo: could use acceleration structure for this if (m_use4componentVertices) { - for (int i=0;i< m_4componentVertices.size();i++) - { - if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold) + if (removeDuplicateVertices) + { + for (int i=0;i< m_4componentVertices.size();i++) { - return i; + if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold) + { + return i; + } } } m_indexedMeshes[0].m_numVertices++; @@ -96,17 +100,20 @@ int btTriangleMesh::findOrAddVertex(const btVector3& vertex) } else { - for (int i=0;i< m_3componentVertices.size();i+=3) + if (removeDuplicateVertices) { - btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]); - if ((vtx-vertex).length2() <= m_weldingThreshold) + for (int i=0;i< m_3componentVertices.size();i+=3) { - return i/3; + btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]); + if ((vtx-vertex).length2() <= m_weldingThreshold) + { + return i/3; + } } - } - m_3componentVertices.push_back(vertex.getX()); - m_3componentVertices.push_back(vertex.getY()); - m_3componentVertices.push_back(vertex.getZ()); + } + m_3componentVertices.push_back((float)vertex.getX()); + m_3componentVertices.push_back((float)vertex.getY()); + m_3componentVertices.push_back((float)vertex.getZ()); m_indexedMeshes[0].m_numVertices++; m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0]; return (m_3componentVertices.size()/3)-1; @@ -114,13 +121,12 @@ int btTriangleMesh::findOrAddVertex(const btVector3& vertex) } -void btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2) +void btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2,bool removeDuplicateVertices) { m_indexedMeshes[0].m_numTriangles++; - - addIndex(findOrAddVertex(vertex0)); - addIndex(findOrAddVertex(vertex1)); - addIndex(findOrAddVertex(vertex2)); + addIndex(findOrAddVertex(vertex0,removeDuplicateVertices)); + addIndex(findOrAddVertex(vertex1,removeDuplicateVertices)); + addIndex(findOrAddVertex(vertex2,removeDuplicateVertices)); } int btTriangleMesh::getNumTriangles() const diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index 1f51b2f2c87..f06fbb7c22d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -25,7 +25,6 @@ subject to the following restrictions: ///It allows either 32bit or 16bit indices, and 4 (x-y-z-w) or 3 (x-y-z) component vertices. ///If you want to share triangle/index data between graphics mesh and collision mesh (btBvhTriangleMeshShape), you can directly use btTriangleIndexVertexArray or derive your own class from btStridingMeshInterface. ///Performance of btTriangleMesh and btTriangleIndexVertexArray used in a btBvhTriangleMeshShape is the same. -///It has a brute-force option to weld together closeby vertices. class btTriangleMesh : public btTriangleIndexVertexArray { btAlignedObjectArray m_4componentVertices; @@ -42,7 +41,7 @@ class btTriangleMesh : public btTriangleIndexVertexArray btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true); - int findOrAddVertex(const btVector3& vertex); + int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices); void addIndex(int index); bool getUse32bitIndices() const @@ -54,8 +53,9 @@ class btTriangleMesh : public btTriangleIndexVertexArray { return m_use4componentVertices; } - - void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2); + ///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes. + ///In general it is better to directly use btTriangleIndexVertexArray instead. + void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2, bool removeDuplicateVertices=false); int getNumTriangles() const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp index 0a2c77096d1..1bd7e84a4e7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -22,8 +22,9 @@ subject to the following restrictions: btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) -: m_meshInterface(meshInterface) +: btConcaveShape (), m_meshInterface(meshInterface) { + m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; if(meshInterface->hasPremadeAabb()) { meshInterface->getPremadeAabb(&m_localAabbMin, &m_localAabbMax); @@ -52,7 +53,7 @@ void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,bt btMatrix3x3 abs_b = trans.getBasis().absolute(); - btPoint3 center = trans(localCenter); + btVector3 center = trans(localCenter); btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), abs_b[1].dot(localHalfExtents), diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index c9eabafe290..53d65799001 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -40,7 +40,7 @@ public: virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const { - assert(0); + btAssert(0); return localGetSupportingVertex(vec); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h index ba7aa0c32a0..4bfb0b9e881 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -40,17 +40,13 @@ public: { vert = m_vertices1[index]; } - virtual int getShapeType() const - { - return TRIANGLE_SHAPE_PROXYTYPE; - } virtual int getNumEdges() const { return 3; } - virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const + virtual void getEdge(int i,btVector3& pa,btVector3& pb) const { getVertex(i,pa); getVertex((i+1)%3,pb); @@ -83,15 +79,16 @@ public: - btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) + btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) : btPolyhedralConvexShape () { + m_shapeType = TRIANGLE_SHAPE_PROXYTYPE; m_vertices1[0] = p0; m_vertices1[1] = p1; m_vertices1[2] = p2; } - virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const { getPlaneEquation(i,planeNormal,planeSupport); } @@ -107,7 +104,7 @@ public: normal.normalize(); } - virtual void getPlaneEquation(int i, btVector3& planeNormal,btPoint3& planeSupport) const + virtual void getPlaneEquation(int i, btVector3& planeNormal,btVector3& planeSupport) const { (void)i; calcNormal(planeNormal); @@ -121,7 +118,7 @@ public: inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } - virtual bool isInside(const btPoint3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt,btScalar tolerance) const { btVector3 normal; calcNormal(normal); @@ -135,7 +132,7 @@ public: int i; for (i=0;i<3;i++) { - btPoint3 pa,pb; + btVector3 pa,pb; getEdge(i,pa,pb); btVector3 edge = pb-pa; btVector3 edgeNormal = edge.cross(normal); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp index ef340286cb0..4c1c716d519 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp @@ -16,9 +16,10 @@ subject to the following restrictions: #include "btUniformScalingShape.h" btUniformScalingShape::btUniformScalingShape( btConvexShape* convexChildShape,btScalar uniformScalingFactor): -m_childConvexShape(convexChildShape), +btConvexShape (), m_childConvexShape(convexChildShape), m_uniformScalingFactor(uniformScalingFactor) { + m_shapeType = UNIFORM_SCALING_SHAPE_PROXYTYPE; } btUniformScalingShape::~btUniformScalingShape() diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h index 1e17fc8e198..945976ac036 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h @@ -61,7 +61,6 @@ class btUniformScalingShape : public btConvexShape return "UniformScalingShape"; } - virtual int getShapeType() const { return UNIFORM_SCALING_SHAPE_PROXYTYPE; } /////////////////////////// diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h index cf43f487e24..827a3c89587 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h @@ -26,9 +26,6 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" -/*! \defgroup BOUND_AABB_OPERATIONS -*/ -//! @{ ///Swap numbers #define BT_SWAP_NUMBERS(a,b){ \ @@ -41,7 +38,7 @@ subject to the following restrictions: #define BT_MAX(a,b) (ab?b:a) -#define BT_GREATER(x, y) fabsf(x) > (y) +#define BT_GREATER(x, y) btFabs(x) > (y) #define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c)) #define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c)) @@ -181,7 +178,7 @@ public: { for(j=0;j<3;j++ ) { - m_AR[i][j] = 1e-6f + fabsf(m_R1to0[i][j]); + m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]); } } @@ -646,6 +643,5 @@ SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btT } -//! @} #endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h index 28c03b7e039..5de391a7561 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h @@ -27,9 +27,6 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btGeometryUtil.h" -/*! \addtogroup GEOMETRIC_OPERATIONS -*/ -//! @{ SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point) { @@ -181,6 +178,5 @@ SIMD_FORCE_INLINE int bt_plane_clip_triangle( -//! @} #endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp index faba2d8a542..c3b697bdd1e 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp @@ -103,12 +103,12 @@ void btContactArray::merge_contacts( push_back(contacts[keycontacts[0].m_value]); - BT_CONTACT * pcontact = &(*this)[0]; + GIM_CONTACT * pcontact = &(*this)[0]; for( i=1;i +class btContactArray:public btAlignedObjectArray { public: btContactArray() @@ -124,11 +120,11 @@ public: const btVector3 &point,const btVector3 & normal, btScalar depth, int feature1, int feature2) { - push_back( BT_CONTACT(point,normal,depth,feature1,feature2) ); + push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) ); } SIMD_FORCE_INLINE void push_triangle_contacts( - const BT_TRIANGLE_CONTACT & tricontact, + const GIM_TRIANGLE_CONTACT & tricontact, int feature1,int feature2) { for(int i = 0;i>1); } - bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); - btAssert(!unbal); + btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); return splitIndex; } -void btBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) { int curIndex = m_num_nodes; m_num_nodes++; @@ -211,7 +210,7 @@ void btBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIn //! stackless build tree void btBvhTree::build_tree( - BT_BVH_DATA_ARRAY & primitive_boxes) + GIM_BVH_DATA_ARRAY & primitive_boxes) { // initialize node count to 0 m_num_nodes = 0; @@ -236,7 +235,7 @@ void btGImpactBvh::refit() } else { - //const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); + //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); //get left bound btAABB bound; bound.invalidate(); @@ -266,7 +265,7 @@ void btGImpactBvh::refit() void btGImpactBvh::buildSet() { //obtain primitive boxes - BT_BVH_DATA_ARRAY primitive_boxes; + GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); for (int i = 0;i +class btPairSet: public btAlignedObjectArray { public: btPairSet() @@ -71,32 +65,32 @@ public: } inline void push_pair(int index1,int index2) { - push_back(BT_PAIR(index1,index2)); + push_back(GIM_PAIR(index1,index2)); } inline void push_pair_inv(int index1,int index2) { - push_back(BT_PAIR(index2,index1)); + push_back(GIM_PAIR(index2,index1)); } }; - -struct BT_BVH_DATA +///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box +struct GIM_BVH_DATA { btAABB m_bound; int m_data; }; //! Node Structure for trees -class BT_BVH_TREE_NODE +class GIM_BVH_TREE_NODE { public: btAABB m_bound; protected: int m_escapeIndexOrDataIndex; public: - BT_BVH_TREE_NODE() + GIM_BVH_TREE_NODE() { m_escapeIndexOrDataIndex = 0; } @@ -133,12 +127,12 @@ public: }; -class BT_BVH_DATA_ARRAY:public btAlignedObjectArray +class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray { }; -class BT_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray +class GIM_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray { }; @@ -150,15 +144,15 @@ class btBvhTree { protected: int m_num_nodes; - BT_BVH_TREE_NODE_ARRAY m_node_array; + GIM_BVH_TREE_NODE_ARRAY m_node_array; protected: int _sort_and_calc_splitting_index( - BT_BVH_DATA_ARRAY & primitive_boxes, + GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex, int splitAxis); - int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); - void _build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); public: btBvhTree() { @@ -167,7 +161,7 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes); + void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); SIMD_FORCE_INLINE void clearNodes() { @@ -218,7 +212,7 @@ public: return m_node_array[nodeindex].getEscapeIndex(); } - SIMD_FORCE_INLINE const BT_BVH_TREE_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const { return &m_node_array[index]; } @@ -382,12 +376,11 @@ public: } - SIMD_FORCE_INLINE const BT_BVH_TREE_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const { return m_box_tree.get_node_pointer(index); } -//! @} static float getAverageTreeCollisionTime(); diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp index 28ad18fbde2..4d6fa88f62d 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp @@ -192,7 +192,7 @@ float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) -: btCollisionAlgorithm(ci) +: btActivatingCollisionAlgorithm(ci,body0,body1) { m_manifoldPtr = NULL; m_convex_algorithm = NULL; @@ -404,7 +404,7 @@ void btGImpactCollisionAlgorithm::collide_sat_triangles(btCollisionObject * body btPrimitiveTriangle ptri0; btPrimitiveTriangle ptri1; - BT_TRIANGLE_CONTACT contact_data; + GIM_TRIANGLE_CONTACT contact_data; shape0->lockChildShapes(); shape1->lockChildShapes(); @@ -540,7 +540,7 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( int i = pairset.size(); while(i--) { - BT_PAIR * pair = &pairset[i]; + GIM_PAIR * pair = &pairset[i]; m_triface0 = pair->m_index1; m_triface1 = pair->m_index2; btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0); @@ -584,14 +584,15 @@ void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0, if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) { btGImpactMeshShape * meshshape0 = static_cast(shape0); - m_part0 = meshshape0->getMeshPartCount(); + int& part = swapped ? m_part1 : m_part0; + part = meshshape0->getMeshPartCount(); - while(m_part0--) + while(part--) { gimpact_vs_shape(body0, body1, - meshshape0->getMeshPart(m_part0), + meshshape0->getMeshPart(part), shape1,swapped); } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h index c745b7ed5d8..453472aa034 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -24,7 +24,7 @@ subject to the following restrictions: #ifndef BVH_CONCAVE_COLLISION_ALGORITHM_H #define BVH_CONCAVE_COLLISION_ALGORITHM_H -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -51,7 +51,7 @@ btCollisionDispatcher * dispatcher = static_cast(m_dyna btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher); \endcode */ -class btGImpactCollisionAlgorithm : public btCollisionAlgorithm +class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm { protected: btCollisionAlgorithm * m_convex_algorithm; @@ -67,7 +67,7 @@ protected: //! Creates a new contact point SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(btCollisionObject* body0,btCollisionObject* body1) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); return m_manifoldPtr; } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp index 0b1ef6acd83..ea1647a8129 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp @@ -67,7 +67,7 @@ float btGImpactQuantizedBvh::getAverageTreeCollisionTime() /////////////////////// btQuantizedBvhTree ///////////////////////////////// void btQuantizedBvhTree::calc_quantization( - BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) + GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) { //calc globa box btAABB global_bound; @@ -86,7 +86,7 @@ void btQuantizedBvhTree::calc_quantization( int btQuantizedBvhTree::_calc_splitting_axis( - BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) + GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) { int i; @@ -118,7 +118,7 @@ int btQuantizedBvhTree::_calc_splitting_axis( int btQuantizedBvhTree::_sort_and_calc_splitting_index( - BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, + GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex, int splitAxis) { int i; @@ -171,15 +171,14 @@ int btQuantizedBvhTree::_sort_and_calc_splitting_index( splitIndex = startIndex+ (numIndices>>1); } - bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); - btAssert(!unbal); + btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); return splitIndex; } -void btQuantizedBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) { int curIndex = m_num_nodes; m_num_nodes++; @@ -232,7 +231,7 @@ void btQuantizedBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, in //! stackless build tree void btQuantizedBvhTree::build_tree( - BT_BVH_DATA_ARRAY & primitive_boxes) + GIM_BVH_DATA_ARRAY & primitive_boxes) { calc_quantization(primitive_boxes); // initialize node count to 0 @@ -258,7 +257,7 @@ void btGImpactQuantizedBvh::refit() } else { - //const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); + //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); //get left bound btAABB bound; bound.invalidate(); @@ -288,7 +287,7 @@ void btGImpactQuantizedBvh::refit() void btGImpactQuantizedBvh::buildSet() { //obtain primitive boxes - BT_BVH_DATA_ARRAY primitive_boxes; + GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); for (int i = 0;i +class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray { }; @@ -112,19 +106,19 @@ class btQuantizedBvhTree { protected: int m_num_nodes; - BT_QUANTIZED_BVH_NODE_ARRAY m_node_array; + GIM_QUANTIZED_BVH_NODE_ARRAY m_node_array; btAABB m_global_bound; btVector3 m_bvhQuantization; protected: - void calc_quantization(BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) ); + void calc_quantization(GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) ); int _sort_and_calc_splitting_index( - BT_BVH_DATA_ARRAY & primitive_boxes, + GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex, int splitAxis); - int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); - void _build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); public: btQuantizedBvhTree() { @@ -133,7 +127,7 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes); + void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); SIMD_FORCE_INLINE void quantizePoint( unsigned short * quantizedpoint, const btVector3 & point) const @@ -365,7 +359,6 @@ public: return m_box_tree.get_node_pointer(index); } -//! @} static float getAverageTreeCollisionTime(); diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h index 43f32bea5a1..05431cfce15 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h @@ -105,6 +105,7 @@ protected: public: btGImpactShapeInterface() { + m_shapeType=GIMPACT_SHAPE_PROXYTYPE; m_localAABB.invalidate(); m_needs_update = true; localScaling.setValue(1.f,1.f,1.f); @@ -880,6 +881,8 @@ Set of btGImpactMeshShapePart parts */ class btGImpactMeshShape : public btGImpactShapeInterface { + btStridingMeshInterface* m_meshInterface; + protected: btAlignedObjectArray m_mesh_parts; void buildMeshParts(btStridingMeshInterface * meshInterface) @@ -906,6 +909,7 @@ protected: public: btGImpactMeshShape(btStridingMeshInterface * meshInterface) { + m_meshInterface = meshInterface; buildMeshParts(meshInterface); } @@ -921,6 +925,15 @@ public: } + btStridingMeshInterface* getMeshInterface() + { + return m_meshInterface; + } + + const btStridingMeshInterface* getMeshInterface() const + { + return m_meshInterface; + } int getMeshPartCount() const { @@ -1032,12 +1045,12 @@ public: } //! call when reading child shapes - virtual void lockChildShapes() + virtual void lockChildShapes() const { btAssert(0); } - virtual void unlockChildShapes() + virtual void unlockChildShapes() const { btAssert(0); } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp index 4b4b5e20dd3..956fa0430e3 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp @@ -251,16 +251,16 @@ bool btGenericPoolAllocator::freeMemory(void * pointer) #define BT_DEFAULT_POOL_ELEMENT_SIZE 8 // main allocator -class BT_STANDARD_ALLOCATOR: public btGenericPoolAllocator +class GIM_STANDARD_ALLOCATOR: public btGenericPoolAllocator { public: - BT_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE) + GIM_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE) { } }; // global allocator -BT_STANDARD_ALLOCATOR g_main_allocator; +GIM_STANDARD_ALLOCATOR g_main_allocator; void * btPoolAlloc(size_t size) diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h index 05b79a82516..e883bcd23d2 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h @@ -155,14 +155,9 @@ public: -/*! \defgroup POOL_MEMORY_FUNCTIONS -standar managed Memory functions. Memory pools are used. -*/ -//! @{ void * btPoolAlloc(size_t size); void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize); void btPoolFree(void *ptr); -//! @} #endif diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h index f5800716a96..bc5a416ddb7 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h @@ -29,9 +29,6 @@ subject to the following restrictions: -/*! \defgroup GEOMETRIC_OPERATIONS -*/ -//! @{ #define PLANEDIREPSILON 0.0000001f @@ -210,7 +207,6 @@ SIMD_FORCE_INLINE void bt_segment_collision( -//! @} #endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h index e9e6ad3d805..7faada61c95 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h @@ -29,9 +29,6 @@ subject to the following restrictions: -/*! \defgroup GEOMETRIC_OPERATIONS -*/ -//! @{ @@ -86,7 +83,6 @@ SIMD_FORCE_INLINE btVector3 bt_unquantize( return vecOut; } -//! @} #endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp index 0267f7e67c7..78696277c34 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp @@ -25,7 +25,7 @@ subject to the following restrictions: -void BT_TRIANGLE_CONTACT::merge_points(const btVector4 & plane, +void GIM_TRIANGLE_CONTACT::merge_points(const btVector4 & plane, btScalar margin, const btVector3 * points, int point_count) { m_point_count = 0; @@ -123,7 +123,7 @@ int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * return clipped_count; } -bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts) +bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts) { btScalar margin = m_margin + other.m_margin; @@ -132,7 +132,7 @@ bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangl //create planes // plane v vs U points - BT_TRIANGLE_CONTACT contacts1; + GIM_TRIANGLE_CONTACT contacts1; contacts1.m_separating_normal = m_plane; @@ -152,7 +152,7 @@ bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangl //Clip tri1 by tri2 edges - BT_TRIANGLE_CONTACT contacts2; + GIM_TRIANGLE_CONTACT contacts2; contacts2.m_separating_normal = other.m_plane; clipped_count = other.clip_triangle(*this,clipped_points); diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h index bdaa323d564..bbd6b630c02 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h @@ -35,14 +35,14 @@ subject to the following restrictions: #define MAX_TRI_CLIPPING 16 //! Structure for collision -struct BT_TRIANGLE_CONTACT +struct GIM_TRIANGLE_CONTACT { btScalar m_penetration_depth; int m_point_count; btVector4 m_separating_normal; btVector3 m_points[MAX_TRI_CLIPPING]; - SIMD_FORCE_INLINE void copy_from(const BT_TRIANGLE_CONTACT& other) + SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT& other) { m_penetration_depth = other.m_penetration_depth; m_separating_normal = other.m_separating_normal; @@ -54,11 +54,11 @@ struct BT_TRIANGLE_CONTACT } } - BT_TRIANGLE_CONTACT() + GIM_TRIANGLE_CONTACT() { } - BT_TRIANGLE_CONTACT(const BT_TRIANGLE_CONTACT& other) + GIM_TRIANGLE_CONTACT(const GIM_TRIANGLE_CONTACT& other) { copy_from(other); } @@ -123,7 +123,7 @@ public: /*! \pre this triangle and other must have their triangles calculated */ - bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts); + bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts); }; diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h index 89fbb5ab0a5..c8161d252d5 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h @@ -34,11 +34,6 @@ email: projectileman@yahoo.com #include "gim_memory.h" -/*! \addtogroup CONTAINERS -\brief -Abstract class for template containers -*/ -//! @{ #define GIM_ARRAY_GROW_INCREMENT 2 #define GIM_ARRAY_GROW_FACTOR 2 @@ -321,11 +316,10 @@ public: { resizeData(m_size); } -//!@} + }; -//! @} diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h index d4cbce04b5e..666abf7917a 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h @@ -40,9 +40,6 @@ email: projectileman@yahoo.com -/*! \defgroup GEOMETRIC_OPERATIONS -*/ -//! @{ #define PLANEDIREPSILON 0.0000001f @@ -541,7 +538,6 @@ SIMD_FORCE_INLINE void SORT_3_INDICES( -//! @} #endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h index 7784aeff140..322004a8d5b 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h @@ -34,11 +34,6 @@ email: projectileman@yahoo.com #include "gim_array.h" -/*! \addtogroup CONTAINERS -\brief -Abstract class for template containers -*/ -//! @{ #define GUINT_BIT_COUNT 32 #define GUINT_EXPONENT 5 @@ -122,7 +117,6 @@ public: }; -//! @} diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h index c9bb83e3653..0add5e4b99f 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h @@ -35,9 +35,6 @@ email: projectileman@yahoo.com #include "gim_basic_geometry_operations.h" #include "LinearMath/btTransform.h" -/*! \defgroup BOUND_AABB_OPERATIONS -*/ -//! @{ //SIMD_FORCE_INLINE bool test_cross_edge_box( @@ -589,6 +586,5 @@ SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btT } -//! @} #endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp index 1ffc2bbad78..0c3d7ba8db0 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp @@ -110,8 +110,7 @@ GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index( splitIndex = startIndex+ (numIndices>>1); } - bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); - btAssert(!unbal); + btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); return splitIndex; } @@ -180,3 +179,4 @@ void GIM_BOX_TREE::build_tree( _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); } + diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h index ece5936de6b..1058a0872a5 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h @@ -40,12 +40,6 @@ email: projectileman@yahoo.com #include "gim_tri_collision.h" -/*! \defgroup BOX_PRUNNING - - - -*/ -//! @{ //! Overlapping pair struct GIM_PAIR @@ -446,7 +440,6 @@ public: m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle); } -//! @} }; //! Class for Box Tree Sets diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h index 62151cb97b7..a91fd3aa422 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h @@ -33,9 +33,6 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ -/*! \addtogroup GEOMETRIC_OPERATIONS -*/ -//! @{ //! This function calcs the distance from a 3D plane class DISTANCE_PLANE_3D_FUNC @@ -209,6 +206,5 @@ SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D( } -//! @} #endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h index 62df51b782f..ad3e12298fe 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h @@ -36,17 +36,6 @@ email: projectileman@yahoo.com #include "gim_radixsort.h" #include "gim_array.h" -/*! \defgroup CONTACTS -\brief -Functions for managing and sorting contacts resulting from a collision query. -
    -
  • Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST -
  • After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY -
  • Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts -
- -*/ -//! @{ /** Configuration var for applying interpolation of contact normals @@ -55,6 +44,10 @@ Configuration var for applying interpolation of contact normals #define CONTACT_DIFF_EPSILON 0.00001f /// Structure for collision results +///Functions for managing and sorting contacts resulting from a collision query. +///Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST +///After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY +///Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts class GIM_CONTACT { public: @@ -168,5 +161,4 @@ public: void merge_contacts_unique(const gim_contact_array & contacts); }; -//! @} #endif // GIM_CONTACT_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h index cd3e53dcc0c..2fb1e3f9e45 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h @@ -36,11 +36,7 @@ email: projectileman@yahoo.com #include "gim_math.h" -/*! \defgroup GEOMETRIC_TYPES -\brief -Basic types and constants for geometry -*/ -//! @{ + //! Short Integer vector 2D typedef GSHORT vec2s[2]; @@ -95,7 +91,6 @@ typedef GREAL mat4f[4][4]; typedef GREAL quatf[4]; //typedef struct _aabb3f aabb3f; -//! @} diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h index fd980dff5fa..93c66f81823 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h @@ -34,11 +34,6 @@ email: projectileman@yahoo.com #include "gim_radixsort.h" -/*! \addtogroup CONTAINERS -\brief -Abstract class for collision objects -*/ -//! @{ #define GIM_INVALID_HASH 0xffffffff //!< A very very high value #define GIM_DEFAULT_HASH_TABLE_SIZE 380 @@ -204,12 +199,7 @@ protected: //SuperBufferedArray< _node_type > m_nodes; bool m_sorted; - /*! \defgroup HASH_TABLE_STRUCTURES - \brief - Hash table data management. The hash table has the indices to the corresponding m_nodes array - */ - //! @{ - + ///Hash table data management. The hash table has the indices to the corresponding m_nodes array GUINT * m_hash_table;//!< GUINT m_table_size;//!< GUINT m_node_size;//!< @@ -459,13 +449,8 @@ protected: } - //! @} - - /*! \defgroup SORTED_ARRAY_STRUCTURES - \brief - Sorted array data management. The hash table has the indices to the corresponding m_nodes array - */ - //! @{ + + ///Sorted array data management. The hash table has the indices to the corresponding m_nodes array inline bool _erase_sorted(GUINT index) { if(index>=(GUINT)m_nodes.size()) return false; @@ -575,8 +560,7 @@ protected: return GIM_INVALID_HASH; } - //! @} - + public: @@ -913,8 +897,6 @@ public: }; -//! @} - #endif // GIM_CONTAINERS_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h index adf1c1c2ddf..0247d4e61cc 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h @@ -40,11 +40,6 @@ email: projectileman@yahoo.com -/*! \defgroup VECTOR_OPERATIONS -T -Operations for vectors : vec2f,vec3f and vec4f -*/ -//! @{ //! Zero out a 2D vector #define VEC_ZERO_2(a) \ @@ -446,13 +441,8 @@ Takes two vectors a, b, blends them together with s <=1 */ -//! @} -/*! \defgroup MATRIX_OPERATIONS -Operations for matrices : mat2f, mat3f and mat4f -*/ -//! @{ /// initialize matrix #define IDENTIFY_MATRIX_3X3(m) \ @@ -1579,6 +1569,5 @@ and m is a mat4f
}\ -//! @} #endif // GIM_VECTOR_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h index 25ad1e21c7f..8b9e6806ef7 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h @@ -35,13 +35,6 @@ email: projectileman@yahoo.com #include "LinearMath/btScalar.h" -/*! \defgroup BASIC_TYPES -Basic types and constants -Conventions: -Types starting with G -Constants starting with G_ -*/ -//! @{ #define GREAL btScalar #define GREAL2 double @@ -52,15 +45,7 @@ Constants starting with G_ #define GINT64 long long #define GUINT64 unsigned long long -//! @} -/*! \defgroup BASIC_CONSTANTS -Basic constants -Conventions: -Types starting with G -Constants starting with G_ -*/ -//! @{ #define G_PI 3.14159265358979f #define G_HALF_PI 1.5707963f @@ -73,14 +58,9 @@ Constants starting with G_ #define G_REAL_INFINITY FLT_MAX #define G_SIGN_BITMASK 0x80000000 #define G_EPSILON SIMD_EPSILON -//! @} -/*! \defgroup SCALAR_TYPES -\brief -Precision type constants -*/ -//! @{ + enum GIM_SCALAR_TYPES { G_STYPE_REAL =0, @@ -92,12 +72,8 @@ enum GIM_SCALAR_TYPES G_STYPE_INT64, G_STYPE_UINT64 }; -//! @} -/*! \defgroup MATH_FUNCTIONS -mathematical functions -*/ -//! @{ + #define G_DEGTORAD(X) ((X)*3.1415926f/180.0f) #define G_RADTODEG(X) ((X)*180.0f/3.1415926f) @@ -131,7 +107,7 @@ mathematical functions ///returns a clamped number #define GIM_CLAMP(number,minval,maxval) (numbermaxval?maxval:number)) -#define GIM_GREATER(x, y) fabsf(x) > (y) +#define GIM_GREATER(x, y) btFabs(x) > (y) ///Swap numbers #define GIM_SWAP_NUMBERS(a,b){ \ @@ -176,6 +152,6 @@ inline GREAL gim_sqrt(GREAL f) return r; } -//! @} + #endif // GIM_MATH_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h index 469a8280f7d..fa99eebb60e 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h @@ -36,9 +36,6 @@ email: projectileman@yahoo.com #include "gim_math.h" #include -//#define PREFETCH 1 -//! \defgroup PREFETCH -//! @{ #ifdef PREFETCH #include // for prefetch #define pfval 64 @@ -53,13 +50,9 @@ email: projectileman@yahoo.com //! Prefetch 128 #define pf2(_x,_i) #endif -//! @} -/*! \defgroup ARRAY_UTILITIES -\brief -Functions for manip packed arrays of numbers -*/ -//! @{ + +///Functions for manip packed arrays of numbers #define GIM_COPY_ARRAYS(dest_array,source_array,element_count)\ {\ for (GUINT _i_=0;_i_*(Ls))(d)); + return(((m_shapes[0])->*(Ls))(d)); } - inline btVector3 Support1(const btVector3& d) const + inline btVector3 Support1(const btVector3& d) const { - return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); + return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); } - inline btVector3 Support(const btVector3& d) const + inline btVector3 Support(const btVector3& d) const { - return(Support0(d)-Support1(-d)); + return(Support0(d)-Support1(-d)); } - btVector3 Support(const btVector3& d,U index) const + btVector3 Support(const btVector3& d,U index) const { - if(index) - return(Support1(d)); + if(index) + return(Support1(d)); else - return(Support0(d)); + return(Support0(d)); } }; -typedef MinkowskiDiff tShape; + typedef MinkowskiDiff tShape; -// GJK -struct GJK -{ -/* Types */ -struct sSV - { - btVector3 d,w; - }; -struct sSimplex - { - sSV* c[4]; - btScalar p[4]; - U rank; - }; -struct eStatus { enum _ { - Valid, - Inside, - Failed };}; -/* Fields */ -tShape m_shape; -btVector3 m_ray; -btScalar m_distance; -sSimplex m_simplices[2]; -sSV m_store[4]; -sSV* m_free[4]; -U m_nfree; -U m_current; -sSimplex* m_simplex; -eStatus::_ m_status; -/* Methods */ - GJK() - { - Initialize(); - } -void Initialize() + // GJK + struct GJK { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; - } -eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) - { - U iterations=0; - btScalar sqdist=0; - btScalar alpha=0; - btVector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eStatus::Valid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar rl=m_ray.length(); - if(rlw; - bool found=false; - for(U i=0;i<4;++i) + /* Types */ + struct sSV + { + btVector3 d,w; + }; + struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; + struct eStatus { enum _ { + Valid, + Inside, + Failed };}; + /* Fields */ + tShape m_shape; + btVector3 m_ray; + btScalar m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + U m_nfree; + U m_current; + sSimplex* m_simplex; + eStatus::_ m_status; + /* Methods */ + GJK() { - if((w-lastw[i]).length2()w, - cs.c[1]->w, - weights,mask);break; - case 3: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - weights,mask);break; - case 4: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - cs.c[3]->w, - weights,mask);break; + m_ray = btVector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; } - if(sqdist>=0) - {/* Valid */ - ns.rank = 0; - m_ray = btVector3(0,0,0); - m_current = next; - for(U i=0,ni=cs.rank;i0?-m_ray:btVector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const btScalar rl=m_ray.length(); + if(rlw; + bool found=false; + for(U i=0;i<4;++i) { - ns.c[ns.rank] = cs.c[i]; - ns.p[ns.rank++] = weights[i]; - m_ray += cs.c[i]->w*weights[i]; + if((w-lastw[i]).length2()w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; } - } - if(mask==15) m_status=eStatus::Inside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)rank) - { - case 1: - { - for(U i=0;i<3;++i) + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = btVector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;iw*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)c[1]->w-m_simplex->c[0]->w; - for(U i=0;i<3;++i) + switch(m_simplex->rank) { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - const btVector3 p=cross(d,axis); - if(p.length2()>0) + case 1: + { + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + const btVector3 p=cross(d,axis); + if(p.length2()>0) + { + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + if(n.length2()>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: { - appendvertice(*m_simplex, p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); + if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + return(true); } + break; } + return(false); } - break; - case 3: + /* Internals */ + void getsupport(const btVector3& d,sSV& sv) const { - const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, - m_simplex->c[2]->w-m_simplex->c[0]->w); - if(n.length2()>0) - { - appendvertice(*m_simplex,n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d); } - break; - case 4: + void removevertice(sSimplex& simplex) { - if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, - m_simplex->c[1]->w-m_simplex->c[3]->w, - m_simplex->c[2]->w-m_simplex->c[3]->w))>0) - return(true); + m_free[m_nfree++]=simplex.c[--simplex.rank]; } - break; - } - return(false); - } -/* Internals */ -void getsupport(const btVector3& d,sSV& sv) const - { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); - } -void removevertice(sSimplex& simplex) - { - m_free[m_nfree++]=simplex.c[--simplex.rank]; - } -void appendvertice(sSimplex& simplex,const btVector3& v) - { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); - } -static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) - { - return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- - a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ - a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); - } -static btScalar projectorigin( const btVector3& a, - const btVector3& b, - btScalar* w,U& m) - { - const btVector3 d=b-a; - const btScalar l=d.length2(); - if(l>GJK_SIMPLEX2_EPS) - { - const btScalar t(l>0?-dot(a,d)/l:0); - if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } - else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } - else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } - } - return(-1); - } -static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar* w,U& m) - { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c}; - const btVector3 dl[]={a-b,b-c,c-a}; - const btVector3 n=cross(dl[0],dl[1]); - const btScalar l=n.length2(); - if(l>GJK_SIMPLEX3_EPS) - { - btScalar mindist=-1; - btScalar subw[2]; - U subm; - for(U i=0;i<3;++i) + void appendvertice(sSimplex& simplex,const btVector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } + static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) { - if(dot(*vt[i],cross(dl[i],n))>0) + return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- + a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ + a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); + } + static btScalar projectorigin( const btVector3& a, + const btVector3& b, + btScalar* w,U& m) + { + const btVector3 d=b-a; + const btScalar l=d.length2(); + if(l>GJK_SIMPLEX2_EPS) { - const U j=imd3[i]; - const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); - if((mindist<0)||(subd(((subm&1)?1<0?-dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } } + return(-1); } - if(mindist<0) + static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w,U& m) { - const btScalar d=dot(a,n); - const btScalar s=btSqrt(l); - const btVector3 p=n*(d/l); - mindist = p.length2(); - m = 7; - w[0] = (cross(dl[1],b-p)).length()/s; - w[1] = (cross(dl[2],c-p)).length()/s; - w[2] = 1-(w[0]+w[1]); + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c}; + const btVector3 dl[]={a-b,b-c,c-a}; + const btVector3 n=cross(dl[0],dl[1]); + const btScalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + btScalar mindist=-1; + btScalar subw[2]; + U subm; + for(U i=0;i<3;++i) + { + if(dot(*vt[i],cross(dl[i],n))>0) + { + const U j=imd3[i]; + const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd(((subm&1)?1<GJK_SIMPLEX4_EPS)) - { - btScalar mindist=-1; - btScalar subw[3]; - U subm; - for(U i=0;i<3;++i) + static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& d, + btScalar* w,U& m) { - const U j=imd3[i]; - const btScalar s=vl*dot(d,cross(dl[i],dl[j])); - if(s>0) + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c,&d}; + const btVector3 dl[]={a-d,b-d,c-d}; + const btScalar vl=det(dl[0],dl[1],dl[2]); + const bool ng=(vl*dot(a,cross(b-c,a-b)))<=0; + if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) { - const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); - if((mindist<0)||(subd((subm&1?1<0) + { + const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd((subm&1?1<e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } -static inline void append(sList& list,sFace* face) + // EPA + struct EPA { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; - } -static inline void remove(sList& list,sFace* face) - { - if(face->l[1]) face->l[1]->l[0]=face->l[0]; - if(face->l[0]) face->l[0]->l[1]=face->l[1]; - if(face==list.root) list.root=face->l[1]; - --list.count; - } - - -void Initialize() - { - m_status = eStatus::Failed; - m_normal = btVector3(0,0,0); - m_depth = 0; - m_nextsv = 0; - for(U i=0;i1)&&gjk.EncloseOrigin()) + btVector3 n; + btScalar d; + btScalar p; + sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + U1 e[3]; + U1 pass; + }; + struct sList + { + sFace* root; + U count; + sList() : root(0),count(0) {} + }; + struct sHorizon { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(0),ff(0),nf(0) {} + }; + struct eStatus { enum _ { + Valid, + Touching, + Degenerated, + NonConvex, + InvalidHull, + OutOfFaces, + OutOfVertices, + AccuraryReached, + FallBack, + Failed };}; + /* Fields */ + eStatus::_ m_status; + GJK::sSimplex m_result; + btVector3 m_normal; + btScalar m_depth; + sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + U m_nextsv; + sList m_hull; + sList m_stock; + /* Methods */ + EPA() + { + Initialize(); + } - /* Clean up */ - while(m_hull.root) + + static inline void bind(sFace* fa,U ea,sFace* fb,U eb) { - sFace* f = m_hull.root; - remove(m_hull,f); - append(m_stock,f); + fa->e[ea]=(U1)eb;fa->f[ea]=fb; + fb->e[eb]=(U1)ea;fb->f[eb]=fa; } - m_status = eStatus::Valid; - m_nextsv = 0; - /* Orient simplex */ - if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, - simplex.c[1]->w-simplex.c[3]->w, - simplex.c[2]->w-simplex.c[3]->w)<0) + static inline void append(sList& list,sFace* face) { - btSwap(simplex.c[0],simplex.c[1]); - btSwap(simplex.p[0],simplex.p[1]); + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; } - /* Build initial hull */ - sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } + + + void Initialize() + { + m_status = eStatus::Failed; + m_normal = btVector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(U i=0;i1)&&gjk.EncloseOrigin()) + { + + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + btSwap(simplex.c[0],simplex.c[1]); + btSwap(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), newface(simplex.c[1],simplex.c[0],simplex.c[3],true), newface(simplex.c[2],simplex.c[1],simplex.c[3],true), newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; - if(m_hull.count==4) - { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U iterations=0; - bind(tetra[0],0,tetra[1],0); - bind(tetra[0],1,tetra[2],0); - bind(tetra[0],2,tetra[3],0); - bind(tetra[1],1,tetra[3],2); - bind(tetra[1],2,tetra[2],1); - bind(tetra[2],2,tetra[3],1); - m_status=eStatus::Valid; - for(;iterationspass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const btScalar wdist=dot(best->n,w->w)-best->d; - if(wdist>EPA_ACCURACY) + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + U pass=0; + U iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterationspass = (U1)(++pass); + gjk.getsupport(best->n,*w); + const btScalar wdist=dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(U j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, best->f[j],best->e[j], horizon); - } - if(valid&&(horizon.nf>=3)) - { - bind(horizon.cf,1,horizon.ff,2); - remove(m_hull,best); - append(m_stock,best); - best=findbest(); - if(best->p>=outer.p) outer=*best; - } else { m_status=eStatus::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + if(best->p>=outer.p) outer=*best; + } else { m_status=eStatus::InvalidHull;break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const btVector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } } - const btVector3 projection=outer.n*outer.d; - m_normal = outer.n; - m_depth = outer.d; - m_result.rank = 3; - m_result.c[0] = outer.c[0]; - m_result.c[1] = outer.c[1]; - m_result.c[2] = outer.c[2]; - m_result.p[0] = cross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = cross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = cross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; - m_result.p[0] /= sum; - m_result.p[1] /= sum; - m_result.p[2] /= sum; - return(m_status); + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const btScalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = btVector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); } - } - /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const btScalar nl=m_normal.length(); - if(nl>0) - m_normal = m_normal/nl; - else - m_normal = btVector3(1,0,0); - m_depth = 0; - m_result.rank=1; - m_result.c[0]=simplex.c[0]; - m_result.p[0]=1; - return(m_status); - } -sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) - { - if(m_stock.root) - { - sFace* face=m_stock.root; - remove(m_stock,face); - append(m_hull,face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = cross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; - face->p = btMin(btMin( - dot(a->w,cross(face->n,a->w-b->w)), - dot(b->w,cross(face->n,b->w-c->w))), - dot(c->w,cross(face->n,c->w-a->w))) / - (v?l:1); - face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; - if(v) + sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) { - face->d = dot(a->w,face->n)/l; - face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) + if(m_stock.root) { - return(face); - } else m_status=eStatus::NonConvex; - } else m_status=eStatus::Degenerated; - remove(m_hull,face); - append(m_stock,face); - return(0); - } - m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; - return(0); - } -sFace* findbest() - { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - btScalar maxp=minf->p; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const btScalar sqd=f->d*f->d; - if((f->p>=maxp)&&(sqdp; + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = cross(b->w-a->w,c->w-a->w); + const btScalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + face->p = btMin(btMin( + dot(a->w,cross(face->n,a->w-b->w)), + dot(b->w,cross(face->n,b->w-c->w))), + dot(c->w,cross(face->n,c->w-a->w))) / + (v?l:1); + face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; + if(v) + { + face->d = dot(a->w,face->n)/l; + face->n /= l; + if(forced||(face->d>=-EPA_PLANE_EPS)) + { + return(face); + } else m_status=eStatus::NonConvex; + } else m_status=eStatus::Degenerated; + remove(m_hull,face); + append(m_stock,face); + return(0); + } + m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; + return(0); } - } - return(minf); - } -bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) - { - static const U i1m3[]={1,2,0}; - static const U i2m3[]={2,0,1}; - if(f->pass!=pass) - { - const U e1=i1m3[e]; - if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + sFace* findbest() { - sFace* nf=newface(f->c[e1],f->c[e],w,false); - if(nf) + sFace* minf=m_hull.root; + btScalar mind=minf->d*minf->d; + btScalar maxp=minf->p; + for(sFace* f=minf->l[1];f;f=f->l[1]) { - bind(nf,0,f,e); - if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; - horizon.cf=nf; - ++horizon.nf; - return(true); + const btScalar sqd=f->d*f->d; + if((f->p>=maxp)&&(sqdp; + } } + return(minf); } - else + bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) { - const U e2=i2m3[e]; - f->pass = (U1)pass; - if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& - expand(pass,w,f->f[e2],f->e[e2],horizon)) + static const U i1m3[]={1,2,0}; + static const U i2m3[]={2,0,1}; + if(f->pass!=pass) { - remove(m_hull,f); - append(m_stock,f); - return(true); + const U e1=i1m3[e]; + if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const U e2=i2m3[e]; + f->pass = (U1)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } } + return(false); } - } - return(false); - } -}; + }; -// -static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - btGjkEpaSolver2::sResults& results, - tShape& shape, - bool withmargins) -{ -/* Results */ -results.witnesses[0] = -results.witnesses[1] = btVector3(0,0,0); -results.status = btGjkEpaSolver2::sResults::Separated; -/* Shape */ -shape.m_shapes[0] = shape0; -shape.m_shapes[1] = shape1; -shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); -shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); -shape.EnableMargin(withmargins); -} + // + static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + btGjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) + { + /* Results */ + results.witnesses[0] = + results.witnesses[1] = btVector3(0,0,0); + results.status = btGjkEpaSolver2::sResults::Separated; + /* Shape */ + shape.m_shapes[0] = shape0; + shape.m_shapes[1] = shape1; + shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); + shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); + shape.EnableMargin(withmargins); + } } @@ -775,87 +775,87 @@ using namespace gjkepa2_impl; // int btGjkEpaSolver2::StackSizeRequirement() { -return(sizeof(GJK)+sizeof(EPA)); + return(sizeof(GJK)+sizeof(EPA)); } // bool btGjkEpaSolver2::Distance( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) { -tShape shape; -Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); -GJK gjk; -GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); -if(gjk_status==GJK::eStatus::Valid) + tShape shape; + Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); + if(gjk_status==GJK::eStatus::Valid) { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;irank;++i) + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;irank;++i) { - const btScalar p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; } - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*w1; - results.normal = w0-w1; - results.distance = results.normal.length(); - results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; - return(true); + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + results.normal = w0-w1; + results.distance = results.normal.length(); + results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; + return(true); } else { - results.status = gjk_status==GJK::eStatus::Inside? - sResults::Penetrating : - sResults::GJK_Failed ; - return(false); + results.status = gjk_status==GJK::eStatus::Inside? + sResults::Penetrating : + sResults::GJK_Failed ; + return(false); } } // bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results, - bool usemargins) + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + bool usemargins) { -tShape shape; -Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); -GJK gjk; -GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); -switch(gjk_status) + tShape shape; + Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); + switch(gjk_status) { case GJK::eStatus::Inside: { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) + EPA epa; + EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=EPA::eStatus::Failed) { - btVector3 w0=btVector3(0,0,0); - for(U i=0;id,0)*epa.m_result.p[i]; + w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; } - results.status = sResults::Penetrating; - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); - results.normal = -epa.m_normal; - results.distance = -epa.m_depth; - return(true); + results.status = sResults::Penetrating; + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return(true); } else results.status=sResults::EPA_Failed; } - break; + break; case GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; + results.status=sResults::GJK_Failed; + break; } -return(false); + return(false); } // @@ -865,49 +865,49 @@ btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, const btTransform& wtrs0, sResults& results) { -tShape shape; -btSphereShape shape1(margin); -btTransform wtrs1(btQuaternion(0,0,0,1),position); -Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); -GJK gjk; -GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); -if(gjk_status==GJK::eStatus::Valid) + tShape shape; + btSphereShape shape1(margin); + btTransform wtrs1(btQuaternion(0,0,0,1),position); + Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); + if(gjk_status==GJK::eStatus::Valid) { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;irank;++i) + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;irank;++i) { - const btScalar p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; } - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*w1; - const btVector3 delta= results.witnesses[1]- - results.witnesses[0]; - const btScalar margin= shape0->getMargin()+ - shape1.getMargin(); - const btScalar length= delta.length(); - results.normal = delta/length; - results.witnesses[0] += results.normal*margin; - return(length-margin); + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + const btVector3 delta= results.witnesses[1]- + results.witnesses[0]; + const btScalar margin= shape0->getMarginNonVirtual()+ + shape1.getMarginNonVirtual(); + const btScalar length= delta.length(); + results.normal = delta/length; + results.witnesses[0] += results.normal*margin; + return(length-margin); } else { - if(gjk_status==GJK::eStatus::Inside) + if(gjk_status==GJK::eStatus::Inside) { - if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) { - const btVector3 delta= results.witnesses[0]- - results.witnesses[1]; - const btScalar length= delta.length(); - if (length >= SIMD_EPSILON) - results.normal = delta/length; - return(-length); + const btVector3 delta= results.witnesses[0]- + results.witnesses[1]; + const btScalar length= delta.length(); + if (length >= SIMD_EPSILON) + results.normal = delta/length; + return(-length); } } } -return(SIMD_INFINITY); + return(SIMD_INFINITY); } // @@ -918,10 +918,10 @@ bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, const btVector3& guess, sResults& results) { -if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) - return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); + if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) + return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); else - return(true); + return(true); } /* Symbols cleanup */ diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index c4f84ed4d75..05573c7cfce 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -17,13 +17,14 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" + + #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, - btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) { @@ -33,20 +34,12 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim const btScalar radialmargin(btScalar(0.)); -//#define USE_ORIGINAL_GJK 1 -#ifdef USE_ORIGINAL_GJK - btGjkEpaSolver::sResults results; - if(btGjkEpaSolver::Collide( pConvexA,transformA, - pConvexB,transformB, - radialmargin,stackAlloc,results)) -#else btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); btGjkEpaSolver2::sResults results; if(btGjkEpaSolver2::Penetration(pConvexA,transformA, pConvexB,transformB, guessVector,results)) -#endif { // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index 2dc069ce5cf..68dbc566518 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -28,7 +28,7 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver bool calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btTransform& transformA, const btTransform& transformB, - btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); private : diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 01fb1a4b068..0856332d1ca 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -18,11 +18,15 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" + + #if defined(DEBUG) || defined (_DEBUG) +//#define TEST_NON_VIRTUAL 1 #include //for debug printf #ifdef __SPU__ #include #define printf spu_printf +//#define DEBUG_SPU_COLLISION_DETECTION 1 #endif //__SPU__ #endif @@ -49,6 +53,8 @@ m_catchDegeneracies(1) void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) { + m_cachedSeparatingDistance = 0.f; + btScalar distance=btScalar(0.); btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 pointOnA,pointOnB; @@ -58,16 +64,35 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; +#ifdef __SPU__ + btScalar marginA = m_minkowskiA->getMarginNonVirtual(); + btScalar marginB = m_minkowskiB->getMarginNonVirtual(); +#else btScalar marginA = m_minkowskiA->getMargin(); btScalar marginB = m_minkowskiB->getMargin(); +#ifdef TEST_NON_VIRTUAL + btScalar marginAv = m_minkowskiA->getMarginNonVirtual(); + btScalar marginBv = m_minkowskiB->getMarginNonVirtual(); + btAssert(marginA == marginAv); + btAssert(marginB == marginBv); +#endif //TEST_NON_VIRTUAL +#endif + + gNumGjkChecks++; +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("inside gjk\n"); +#endif //for CCD we don't use margins if (m_ignoreMargin) { marginA = btScalar(0.); marginB = btScalar(0.); +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("ignoring margin\n"); +#endif } m_curIter = 0; @@ -98,18 +123,35 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); +#ifdef __SPU__ + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); +#else btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); - btPoint3 pWorld = localTransA(pInA); - btPoint3 qWorld = localTransB(qInB); - +#ifdef TEST_NON_VIRTUAL + btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); + btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); + btAssert((pInAv-pInA).length() < 0.0001); + btAssert((qInBv-qInB).length() < 0.0001); +#endif // +#endif //__SPU__ + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("got local supporting vertices\n"); +#endif + btVector3 w = pWorld - qWorld; delta = m_cachedSeparatingAxis.dot(w); // potential exit, they don't overlap if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) { - checkPenetration = false; + checkSimplex=true; + //checkPenetration = false; break; } @@ -133,9 +175,15 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& checkSimplex = true; break; } + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("addVertex 1\n"); +#endif //add current vertex to simplex m_simplexSolver->addVertex(w, pWorld, qWorld); - +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("addVertex 2\n"); +#endif //calculate the closest point to the origin (update vector v) if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) { @@ -167,7 +215,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject if (m_curIter++ > gGjkMaxIter) { - #if defined(DEBUG) || defined (_DEBUG) + #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION) printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", @@ -290,10 +338,20 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& #endif //__CELLOS_LV2__ +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("output 1\n"); +#endif + m_cachedSeparatingAxis = normalInB; + m_cachedSeparatingDistance = distance; + output.addContactPoint( normalInB, pointOnB+positionOffset, distance); + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("output 2\n"); +#endif //printf("gjk add:%f",distance); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index 550fc4677e0..0ad4aab8a59 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -20,7 +20,6 @@ subject to the following restrictions: #define GJK_PAIR_DETECTOR_H #include "btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btPoint3.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" class btConvexShape; @@ -38,6 +37,7 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface const btConvexShape* m_minkowskiA; const btConvexShape* m_minkowskiB; bool m_ignoreMargin; + btScalar m_cachedSeparatingDistance; public: @@ -68,6 +68,15 @@ public: m_cachedSeparatingAxis = seperatingAxis; } + const btVector3& getCachedSeparatingAxis() const + { + return m_cachedSeparatingAxis; + } + btScalar getCachedSeparatingDistance() const + { + return m_cachedSeparatingDistance; + } + void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) { m_penetrationDepthSolver = penetrationDepthSolver; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index e75fc1bee96..c7c9812985d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -32,6 +32,8 @@ class btManifoldPoint :m_userPersistentData(0), m_appliedImpulse(0.f), m_lateralFrictionInitialized(false), + m_appliedImpulseLateral1(0.f), + m_appliedImpulseLateral2(0.f), m_lifeTime(0) { } @@ -110,6 +112,12 @@ class btManifoldPoint m_distance1 = dist; } + ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver + btScalar getAppliedImpulse() const + { + return m_appliedImpulse; + } + }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index 0e6fa2e6dfe..581b4258f03 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -19,9 +19,6 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" - - - #define NUM_UNITSPHERE_POINTS 42 static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = { @@ -73,7 +70,7 @@ btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, - btVector3& v, btPoint3& pa, btPoint3& pb, + btVector3& v, btVector3& pa, btVector3& pb, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ) { @@ -117,7 +114,9 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 seperatingAxisInA,seperatingAxisInB; btVector3 pInA,qInB,pWorld,qWorld,w; +#ifndef __SPU__ #define USE_BATCHED_SUPPORT 1 +#endif #ifdef USE_BATCHED_SUPPORT btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; @@ -200,6 +199,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numSampleDirections = NUM_UNITSPHERE_POINTS; +#ifndef __SPU__ { int numPDA = convexA->getNumPreferredPenetrationDirections(); if (numPDA) @@ -229,14 +229,15 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s } } } +#endif // __SPU__ for (int i=0;ilocalGetSupportingVertexWithoutMargin(seperatingAxisInA); - qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); + pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); pWorld = transA(pInA); qWorld = transB(qInB); w = qWorld - pWorld; @@ -254,13 +255,13 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s //add the margins - minA += minNorm*convexA->getMargin(); - minB -= minNorm*convexB->getMargin(); + minA += minNorm*convexA->getMarginNonVirtual(); + minB -= minNorm*convexB->getMarginNonVirtual(); //no penetration if (minProj < btScalar(0.)) return false; - minProj += (convexA->getMargin() + convexB->getMargin()); + minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h index 27b42c2b47e..23cbd57ac7e 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -27,10 +27,9 @@ public: virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, const btConvexShape* convexA,const btConvexShape* convexB, const btTransform& transA,const btTransform& transB, - btVector3& v, btPoint3& pa, btPoint3& pb, + btVector3& v, btVector3& pa, btVector3& pb, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc ); - }; #endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 386885d2ac8..d4e29882d37 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btPersistentManifold.h" #include "LinearMath/btTransform.h" -#include + btScalar gContactBreakingThreshold = btScalar(0.02); ContactDestroyedCallback gContactDestroyedCallback = 0; @@ -66,7 +66,7 @@ void btPersistentManifold::clearUserCache(btManifoldPoint& pt) printf("error in clearUserCache\n"); } } - assert(occurance<=0); + btAssert(occurance<=0); #endif //DEBUG_PERSISTENCY if (pt.m_userPersistentData && gContactDestroyedCallback) @@ -164,7 +164,7 @@ int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) { - assert(validContactDistance(newPoint)); + btAssert(validContactDistance(newPoint)); int insertIndex = getNumContacts(); if (insertIndex == MANIFOLD_CACHE_SIZE) @@ -183,6 +183,9 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) } + if (insertIndex<0) + insertIndex=0; + btAssert(m_pointCache[insertIndex].m_userPersistentData==0); m_pointCache[insertIndex] = newPoint; return insertIndex; @@ -190,7 +193,7 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) btScalar btPersistentManifold::getContactBreakingThreshold() const { - return gContactBreakingThreshold; + return m_contactBreakingThreshold; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index c122eb865e8..0b3c734d1d7 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -24,7 +24,7 @@ subject to the following restrictions: struct btCollisionResult; -///contact breaking and merging threshold +///maximum contact breaking and merging threshold extern btScalar gContactBreakingThreshold; typedef bool (*ContactDestroyedCallback)(void* userPersistentData); @@ -54,6 +54,9 @@ ATTRIBUTE_ALIGNED16( class) btPersistentManifold void* m_body1; int m_cachedPoints; + btScalar m_contactBreakingThreshold; + btScalar m_contactProcessingThreshold; + /// sort cached points so most isolated points come first int sortCachedPoints(const btManifoldPoint& pt); @@ -68,10 +71,12 @@ public: btPersistentManifold(); - btPersistentManifold(void* body0,void* body1,int bla) - : m_body0(body0),m_body1(body1),m_cachedPoints(0) + btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold) + : m_body0(body0),m_body1(body1),m_cachedPoints(0), + m_contactBreakingThreshold(contactBreakingThreshold), + m_contactProcessingThreshold(contactProcessingThreshold) { - (void)bla; + } SIMD_FORCE_INLINE void* getBody0() { return m_body0;} @@ -106,8 +111,13 @@ public: return m_pointCache[index]; } - /// todo: get this margin from the current physics / collision environment + ///@todo: get this margin from the current physics / collision environment btScalar getContactBreakingThreshold() const; + + btScalar getContactProcessingThreshold() const + { + return m_contactProcessingThreshold; + } int getCacheEntry(const btManifoldPoint& newPoint) const; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index a70a3ae56c4..cdb1d22444d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -23,10 +23,12 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "btRaycastCallback.h" -btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to) +btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags) : m_from(from), m_to(to), + //@BP Mod + m_flags(flags), m_hitFraction(btScalar(1.)) { @@ -55,6 +57,12 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, { return ; // same sign } + //@BP Mod - Backface filtering + if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a > btScalar(0.0))) + { + // Backface, skip check + return; + } const btScalar proj_length=dist_a-dist_b; const btScalar distance = (dist_a)/(proj_length); @@ -89,14 +97,18 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) { + //@BP Mod + // Triangle normal isn't normalized + triangleNormal.normalize(); - if ( dist_a > 0 ) + //@BP Mod - Allow for unflipped normal when raycasting against backfaces + if (((m_flags & kF_KeepUnflippedNormal) != 0) || (dist_a <= btScalar(0.0))) { - m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex); + m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex); } else { - m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex); + m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex); } } } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index d2b4b80f8ba..3a1ab388c13 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -29,9 +29,20 @@ public: btVector3 m_from; btVector3 m_to; + //@BP Mod - allow backface filtering and unflipped normals + enum EFlags + { + kF_None = 0, + kF_FilterBackfaces = 1 << 0, + kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle + + kF_Terminator = 0xFFFFFFFF + }; + unsigned int m_flags; + btScalar m_hitFraction; - btTriangleRaycastCallback(const btVector3& from,const btVector3& to); + btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h index cf65f46505b..823b4e7158b 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -19,7 +19,6 @@ subject to the following restrictions: #define SIMPLEX_SOLVER_INTERFACE_H #include "LinearMath/btVector3.h" -#include "LinearMath/btPoint3.h" #define NO_VIRTUAL_INTERFACE 1 #ifdef NO_VIRTUAL_INTERFACE @@ -37,7 +36,7 @@ class btSimplexSolverInterface virtual void reset() = 0; - virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) = 0; + virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0; virtual bool closest(btVector3& v) = 0; @@ -45,7 +44,7 @@ class btSimplexSolverInterface virtual bool fullSimplex() const = 0; - virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const = 0; + virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0; virtual bool inSimplex(const btVector3& w) = 0; @@ -53,7 +52,7 @@ class btSimplexSolverInterface virtual bool emptySimplex() const = 0; - virtual void compute_points(btPoint3& p1, btPoint3& p2) = 0; + virtual void compute_points(btVector3& p1, btVector3& p2) = 0; virtual int numVertices() const =0; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index cf8a3ab5eb1..a7ffeda8c62 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -25,8 +25,6 @@ subject to the following restrictions: #include "btVoronoiSimplexSolver.h" -#include -//#include #define VERTA 0 #define VERTB 1 @@ -37,7 +35,7 @@ subject to the following restrictions: void btVoronoiSimplexSolver::removeVertex(int index) { - assert(m_numVertices>0); + btAssert(m_numVertices>0); m_numVertices--; m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; @@ -77,7 +75,7 @@ void btVoronoiSimplexSolver::reset() //add a vertex -void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) +void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q) { m_lastW = w; m_needsUpdate = true; @@ -267,7 +265,7 @@ btScalar btVoronoiSimplexSolver::maxVertex() //return the current simplex -int btVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const +int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const { int i; for (i=0;i +//----------------------------------------------------------------------------- + +#define CONETWIST_USE_OBSOLETE_SOLVER false +#define CONETWIST_DEF_FIX_THRESH btScalar(.05f) + +//----------------------------------------------------------------------------- + btConeTwistConstraint::btConeTwistConstraint() -:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE) +:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE), +m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { } @@ -31,69 +39,228 @@ btConeTwistConstraint::btConeTwistConstraint() btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame,const btTransform& rbBFrame) :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), - m_angularOnly(false) + m_angularOnly(false), + m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { - m_swingSpan1 = btScalar(1e30); - m_swingSpan2 = btScalar(1e30); - m_twistSpan = btScalar(1e30); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - - m_solveTwistLimit = false; - m_solveSwingLimit = false; - + init(); } btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), - m_angularOnly(false) + m_angularOnly(false), + m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { m_rbBFrame = m_rbAFrame; - - m_swingSpan1 = btScalar(1e30); - m_swingSpan2 = btScalar(1e30); - m_twistSpan = btScalar(1e30); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; + init(); +} - m_solveTwistLimit = false; - m_solveSwingLimit = false; - -} -void btConeTwistConstraint::buildJacobian() +void btConeTwistConstraint::init() { - m_appliedImpulse = btScalar(0.); - - //set bias, sign, clear accumulator - m_swingCorrection = btScalar(0.); - m_twistLimitSign = btScalar(0.); + m_angularOnly = false; m_solveTwistLimit = false; m_solveSwingLimit = false; - m_accTwistLimitImpulse = btScalar(0.); - m_accSwingLimitImpulse = btScalar(0.); + m_bMotorEnabled = false; + m_maxMotorImpulse = btScalar(-1); + + setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30)); + m_damping = btScalar(0.01); + m_fixThresh = CONETWIST_DEF_FIX_THRESH; +} - if (!m_angularOnly) + +//----------------------------------------------------------------------------- + +void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; + info->m_numConstraintRows = 0; + info->nub = 0; + } + else + { + info->m_numConstraintRows = 3; + info->nub = 3; + calcAngleInfo2(); + if(m_solveSwingLimit) + { + info->m_numConstraintRows++; + info->nub--; + if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + { + info->m_numConstraintRows++; + info->nub--; + } + } + if(m_solveTwistLimit) + { + info->m_numConstraintRows++; + info->nub--; + } + } +} // btConeTwistConstraint::getInfo1() + +//----------------------------------------------------------------------------- - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) +void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + //retrieve matrices + btTransform body0_trans; + body0_trans = m_rbA.getCenterOfMassTransform(); + btTransform body1_trans; + body1_trans = m_rbB.getCenterOfMassTransform(); + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; + btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + // set right hand side + btScalar k = info->fps * info->erp; + int j; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY; + info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY; + } + int row = 3; + int srow = row * info->rowskip; + btVector3 ax1; + // angular limits + if(m_solveSwingLimit) + { + btScalar *J1 = info->m_J1angularAxis; + btScalar *J2 = info->m_J2angularAxis; + if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + { + btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; + btVector3 p = trA.getBasis().getColumn(1); + btVector3 q = trA.getBasis().getColumn(2); + int srow1 = srow + info->rowskip; + J1[srow+0] = p[0]; + J1[srow+1] = p[1]; + J1[srow+2] = p[2]; + J1[srow1+0] = q[0]; + J1[srow1+1] = q[1]; + J1[srow1+2] = q[2]; + J2[srow+0] = -p[0]; + J2[srow+1] = -p[1]; + J2[srow+2] = -p[2]; + J2[srow1+0] = -q[0]; + J2[srow1+1] = -q[1]; + J2[srow1+2] = -q[2]; + btScalar fact = info->fps * m_relaxationFactor; + info->m_constraintError[srow] = fact * m_swingAxis.dot(p); + info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + info->m_lowerLimit[srow1] = -SIMD_INFINITY; + info->m_upperLimit[srow1] = SIMD_INFINITY; + srow = srow1 + info->rowskip; + } + else + { + ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + btScalar k = info->fps * m_biasFactor; + + info->m_constraintError[srow] = k * m_swingCorrection; + info->cfm[srow] = 0.0f; + // m_swingCorrection is always positive or 0 + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + srow += info->rowskip; + } + } + if(m_solveTwistLimit) + { + ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; + btScalar *J1 = info->m_J1angularAxis; + btScalar *J2 = info->m_J2angularAxis; + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + btScalar k = info->fps * m_biasFactor; + info->m_constraintError[srow] = k * m_twistCorrection; + info->cfm[srow] = 0.0f; + if(m_twistSpan > 0.0f) { - normal[0] = relPos.normalized(); + + if(m_twistCorrection > 0.0f) + { + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } } else { - normal[0].setValue(btScalar(1.0),0,0); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; } + srow += info->rowskip; + } +} + +//----------------------------------------------------------------------------- - btPlaneSpace1(normal[0], normal[1], normal[2]); +void btConeTwistConstraint::buildJacobian() +{ + if (m_useSolveConstraintObsolete) + { + m_appliedImpulse = btScalar(0.); + m_accTwistLimitImpulse = btScalar(0.); + m_accSwingLimitImpulse = btScalar(0.); - for (int i=0;i<3;i++) + if (!m_angularOnly) { - new (&m_jac[i]) btJacobianEntry( + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 relPos = pivotBInW - pivotAInW; + + btVector3 normal[3]; + if (relPos.length2() > SIMD_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0].setValue(btScalar(1.0),0,0); + } + + btPlaneSpace1(normal[0], normal[1], normal[2]); + + for (int i=0;i<3;i++) + { + new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), @@ -103,9 +270,243 @@ void btConeTwistConstraint::buildJacobian() m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); + } + } + + calcAngleInfo2(); + } +} + +//----------------------------------------------------------------------------- + +void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + + btScalar tau = btScalar(0.3); + + //linear part + if (!m_angularOnly) + { + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + btVector3 vel2; + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; + + for (int i=0;i<3;i++) + { + const btVector3& normal = m_jac[i].m_linearJointAxis; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + + btScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); + btVector3 ftorqueAxis2 = rel_pos2.cross(normal); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); + + } + } + + // apply motor + if (m_bMotorEnabled) + { + // compute current and predicted transforms + btTransform trACur = m_rbA.getCenterOfMassTransform(); + btTransform trBCur = m_rbB.getCenterOfMassTransform(); + btVector3 omegaA; bodyA.getAngularVelocity(omegaA); + btVector3 omegaB; bodyB.getAngularVelocity(omegaB); + btTransform trAPred; trAPred.setIdentity(); + btVector3 zerovec(0,0,0); + btTransformUtil::integrateTransform( + trACur, zerovec, omegaA, timeStep, trAPred); + btTransform trBPred; trBPred.setIdentity(); + btTransformUtil::integrateTransform( + trBCur, zerovec, omegaB, timeStep, trBPred); + + // compute desired transforms in world + btTransform trPose(m_qTarget); + btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse(); + btTransform trADes = trBPred * trABDes; + btTransform trBDes = trAPred * trABDes.inverse(); + + // compute desired omegas in world + btVector3 omegaADes, omegaBDes; + + btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); + btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); + + // compute delta omegas + btVector3 dOmegaA = omegaADes - omegaA; + btVector3 dOmegaB = omegaBDes - omegaB; + + // compute weighted avg axis of dOmega (weighting based on inertias) + btVector3 axisA, axisB; + btScalar kAxisAInv = 0, kAxisBInv = 0; + + if (dOmegaA.length2() > SIMD_EPSILON) + { + axisA = dOmegaA.normalized(); + kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA); + } + + if (dOmegaB.length2() > SIMD_EPSILON) + { + axisB = dOmegaB.normalized(); + kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB); + } + + btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; + + static bool bDoTorque = true; + if (bDoTorque && avgAxis.length2() > SIMD_EPSILON) + { + avgAxis.normalize(); + kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis); + kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis); + btScalar kInvCombined = kAxisAInv + kAxisBInv; + + btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) / + (kInvCombined * kInvCombined); + + if (m_maxMotorImpulse >= 0) + { + btScalar fMaxImpulse = m_maxMotorImpulse; + if (m_bNormalizedMotorStrength) + fMaxImpulse = fMaxImpulse/kAxisAInv; + + btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; + btScalar newUnclampedMag = newUnclampedAccImpulse.length(); + if (newUnclampedMag > fMaxImpulse) + { + newUnclampedAccImpulse.normalize(); + newUnclampedAccImpulse *= fMaxImpulse; + impulse = newUnclampedAccImpulse - m_accMotorImpulse; + } + m_accMotorImpulse += impulse; + } + + btScalar impulseMag = impulse.length(); + btVector3 impulseAxis = impulse / impulseMag; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + + } + } + else // no motor: do a little damping + { + const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); + const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + btVector3 relVel = angVelB - angVelA; + if (relVel.length2() > SIMD_EPSILON) + { + btVector3 relVelAxis = relVel.normalized(); + btScalar m_kDamping = btScalar(1.) / + (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + + getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); + btVector3 impulse = m_damping * m_kDamping * relVel; + + btScalar impulseMag = impulse.length(); + btVector3 impulseAxis = impulse / impulseMag; + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + } + } + + // joint limits + { + ///solve angular part + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); + + // solve swing limit + if (m_solveSwingLimit) + { + btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; + btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); + if (relSwingVel > 0) + amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; + btScalar impulseMag = amplitude * m_kSwing; + + // Clamp the accumulated impulse + btScalar temp = m_accSwingLimitImpulse; + m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); + impulseMag = m_accSwingLimitImpulse - temp; + + btVector3 impulse = m_swingAxis * impulseMag; + + // don't let cone response affect twist + // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit) + { + btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA; + btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple; + impulse = impulseNoTwistCouple; + } + + impulseMag = impulse.length(); + btVector3 noTwistSwingAxis = impulse / impulseMag; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); + } + + + // solve twist limit + if (m_solveTwistLimit) + { + btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep; + btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis ); + if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) + amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; + btScalar impulseMag = amplitude * m_kTwist; + + // Clamp the accumulated impulse + btScalar temp = m_accTwistLimitImpulse; + m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); + impulseMag = m_accTwistLimitImpulse - temp; + + btVector3 impulse = m_twistAxis * impulseMag; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); + } } } +} + +//----------------------------------------------------------------------------- + +void btConeTwistConstraint::updateRHS(btScalar timeStep) +{ + (void)timeStep; + +} + +//----------------------------------------------------------------------------- + +void btConeTwistConstraint::calcAngleInfo() +{ + m_swingCorrection = btScalar(0.); + m_twistLimitSign = btScalar(0.); + m_solveTwistLimit = false; + m_solveSwingLimit = false; + btVector3 b1Axis1,b1Axis2,b1Axis3; btVector3 b2Axis1,b2Axis2; @@ -122,20 +523,17 @@ void btConeTwistConstraint::buildJacobian() if (m_swingSpan1 >= btScalar(0.05f)) { b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); -// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis2); swing1 = btAtan2Fast(swy, swx); fact = (swy*swy + swx*swx) * thresh * thresh; fact = fact / (fact + btScalar(1.0)); swing1 *= fact; - } if (m_swingSpan2 >= btScalar(0.05f)) { b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); -// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); swx = b2Axis1.dot(b1Axis1); swy = b2Axis1.dot(b1Axis3); swing2 = btAtan2Fast(swy, swx); @@ -152,17 +550,11 @@ void btConeTwistConstraint::buildJacobian() { m_swingCorrection = EllipseAngle-1.0f; m_solveSwingLimit = true; - // Calculate necessary axis & factors m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); m_swingAxis.normalize(); - btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; - - m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); - } // Twist limits @@ -172,118 +564,426 @@ void btConeTwistConstraint::buildJacobian() btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); + m_twistAngle = twist; - btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); +// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); + btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); if (twist <= -m_twistSpan*lockedFreeFactor) { m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); m_twistAxis *= -1.0f; + } + else if (twist > m_twistSpan*lockedFreeFactor) + { + m_twistCorrection = (twist - m_twistSpan); + m_solveTwistLimit = true; + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + } + } +} // btConeTwistConstraint::calcAngleInfo() + + +static btVector3 vTwist(1,0,0); // twist axis in constraint's space + +//----------------------------------------------------------------------------- + +void btConeTwistConstraint::calcAngleInfo2() +{ + m_swingCorrection = btScalar(0.); + m_twistLimitSign = btScalar(0.); + m_solveTwistLimit = false; + m_solveSwingLimit = false; + + { + // compute rotation of A wrt B (in constraint space) + btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation(); + btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation(); + btQuaternion qAB = qB.inverse() * qA; + + // split rotation into cone and twist + // (all this is done from B's perspective. Maybe I should be averaging axes...) + btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); + btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); + btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); + + if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) + { + btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; + computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); + + if (swingAngle > swingLimit * m_limitSoftness) + { + m_solveSwingLimit = true; + + // compute limit ratio: 0->1, where + // 0 == beginning of soft limit + // 1 == hard/real limit + m_swingLimitRatio = 1.f; + if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) + { + m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ + (swingLimit - swingLimit * m_limitSoftness); + } + + // swing correction tries to get back to soft limit + m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); + + // adjustment of swing axis (based on ellipse normal) + adjustSwingAxisToUseEllipseNormal(swingAxis); + + // Calculate necessary axis & factors + m_swingAxis = quatRotate(qB, -swingAxis); + + m_twistAxisA.setValue(0,0,0); + + m_kSwing = btScalar(1.) / + (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + + getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); + } + } + else + { + // you haven't set any limits; + // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) + // anyway, we have either hinge or fixed joint + btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); + btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); + btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0); + btVector3 target; + btScalar x = ivB.dot(ivA); + btScalar y = ivB.dot(jvA); + btScalar z = ivB.dot(kvA); + if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) + { // fixed. We'll need to add one more row to constraint + if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) + { + m_solveSwingLimit = true; + m_swingAxis = -ivB.cross(ivA); + } + } + else + { + if(m_swingSpan1 < m_fixThresh) + { // hinge around Y axis + if(!(btFuzzyZero(y))) + { + m_solveSwingLimit = true; + if(m_swingSpan2 >= m_fixThresh) + { + y = btScalar(0.f); + btScalar span2 = btAtan2(z, x); + if(span2 > m_swingSpan2) + { + x = btCos(m_swingSpan2); + z = btSin(m_swingSpan2); + } + else if(span2 < -m_swingSpan2) + { + x = btCos(m_swingSpan2); + z = -btSin(m_swingSpan2); + } + } + } + } + else + { // hinge around Z axis + if(!btFuzzyZero(z)) + { + m_solveSwingLimit = true; + if(m_swingSpan1 >= m_fixThresh) + { + z = btScalar(0.f); + btScalar span1 = btAtan2(y, x); + if(span1 > m_swingSpan1) + { + x = btCos(m_swingSpan1); + y = btSin(m_swingSpan1); + } + else if(span1 < -m_swingSpan1) + { + x = btCos(m_swingSpan1); + y = -btSin(m_swingSpan1); + } + } + } + } + target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0]; + target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1]; + target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; + target.normalize(); + m_swingAxis = -ivB.cross(target); + m_swingCorrection = m_swingAxis.length(); + m_swingAxis.normalize(); + } + } - m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); + if (m_twistSpan >= btScalar(0.f)) + { + btVector3 twistAxis; + computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); - } else - if (twist > m_twistSpan*lockedFreeFactor) + if (m_twistAngle > m_twistSpan*m_limitSoftness) { - m_twistCorrection = (twist - m_twistSpan); m_solveTwistLimit = true; - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); + m_twistLimitRatio = 1.f; + if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) + { + m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ + (m_twistSpan - m_twistSpan * m_limitSoftness); + } - m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); + // twist correction tries to get back to soft limit + m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness); + m_twistAxis = quatRotate(qB, -twistAxis); + + m_kTwist = btScalar(1.) / + (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + + getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); } + + if (m_solveSwingLimit) + m_twistAxisA = quatRotate(qA, -twistAxis); + } + else + { + m_twistAngle = btScalar(0.f); + } + } +} // btConeTwistConstraint::calcAngleInfo2() + + + +// given a cone rotation in constraint space, (pre: twist must already be removed) +// this method computes its corresponding swing angle and axis. +// more interestingly, it computes the cone/swing limit (angle) for this cone "pose". +void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, + btScalar& swingAngle, // out + btVector3& vSwingAxis, // out + btScalar& swingLimit) // out +{ + swingAngle = qCone.getAngle(); + if (swingAngle > SIMD_EPSILON) + { + vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z()); + vSwingAxis.normalize(); + if (fabs(vSwingAxis.x()) > SIMD_EPSILON) + { + // non-zero twist?! this should never happen. + int wtf = 0; wtf = wtf; + } + + // Compute limit for given swing. tricky: + // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. + // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) + + // For starters, compute the direction from center to surface of ellipse. + // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. + // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) + btScalar xEllipse = vSwingAxis.y(); + btScalar yEllipse = -vSwingAxis.z(); + + // Now, we use the slope of the vector (using x/yEllipse) and find the length + // of the line that intersects the ellipse: + // x^2 y^2 + // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) + // a^2 b^2 + // Do the math and it should be clear. + + swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 + if (fabs(xEllipse) > SIMD_EPSILON) + { + btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); + btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); + norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); + btScalar swingLimit2 = (1 + surfaceSlope2) / norm; + swingLimit = sqrt(swingLimit2); + } + + // test! + /*swingLimit = m_swingSpan2; + if (fabs(vSwingAxis.z()) > SIMD_EPSILON) + { + btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2; + btScalar sinphi = m_swingSpan2 / sqrt(mag_2); + btScalar phi = asin(sinphi); + btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z())); + btScalar alpha = 3.14159f - theta - phi; + btScalar sinalpha = sin(alpha); + swingLimit = m_swingSpan1 * sinphi/sinalpha; + }*/ + } + else if (swingAngle < 0) + { + // this should never happen! + int wtf = 0; wtf = wtf; } } -void btConeTwistConstraint::solveConstraint(btScalar timeStep) +btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const { + // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone) + btScalar xEllipse = btCos(fAngleInRadians); + btScalar yEllipse = btSin(fAngleInRadians); + + // Use the slope of the vector (using x/yEllipse) and find the length + // of the line that intersects the ellipse: + // x^2 y^2 + // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) + // a^2 b^2 + // Do the math and it should be clear. + + float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) + if (fabs(xEllipse) > SIMD_EPSILON) + { + btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); + btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); + norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); + btScalar swingLimit2 = (1 + surfaceSlope2) / norm; + swingLimit = sqrt(swingLimit2); + } - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + // convert into point in constraint space: + // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively + btVector3 vSwingAxis(0, xEllipse, -yEllipse); + btQuaternion qSwing(vSwingAxis, swingLimit); + btVector3 vPointInConstraintSpace(fLength,0,0); + return quatRotate(qSwing, vPointInConstraintSpace); +} - btScalar tau = btScalar(0.3); +// given a twist rotation in constraint space, (pre: cone must already be removed) +// this method computes its corresponding angle and axis. +void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, + btScalar& twistAngle, // out + btVector3& vTwistAxis) // out +{ + btQuaternion qMinTwist = qTwist; + twistAngle = qTwist.getAngle(); - //linear part - if (!m_angularOnly) + if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - for (int i=0;i<3;i++) - { - const btVector3& normal = m_jac[i].m_linearJointAxis; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btScalar rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); - } + qMinTwist = operator-(qTwist); + twistAngle = qMinTwist.getAngle(); } - + if (twistAngle < 0) { - ///solve angular part - const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); - const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + // this should never happen + int wtf = 0; wtf = wtf; + } - // solve swing limit - if (m_solveSwingLimit) - { - btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor); - btScalar impulseMag = amplitude * m_kSwing; + vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); + if (twistAngle > SIMD_EPSILON) + vTwistAxis.normalize(); +} - // Clamp the accumulated impulse - btScalar temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accSwingLimitImpulse - temp; - btVector3 impulse = m_swingAxis * impulseMag; +void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const +{ + // the swing axis is computed as the "twist-free" cone rotation, + // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). + // so, if we're outside the limits, the closest way back inside the cone isn't + // along the vector back to the center. better (and more stable) to use the ellipse normal. + + // convert swing axis to direction from center to surface of ellipse + // (ie. rotate 2D vector by PI/2) + btScalar y = -vSwingAxis.z(); + btScalar z = vSwingAxis.y(); + + // do the math... + if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. + { + // compute gradient/normal of ellipse surface at current "point" + btScalar grad = y/z; + grad *= m_swingSpan2 / m_swingSpan1; - m_rbA.applyTorqueImpulse(impulse); - m_rbB.applyTorqueImpulse(-impulse); + // adjust y/z to represent normal at point (instead of vector to point) + if (y > 0) + y = fabs(grad * z); + else + y = -fabs(grad * z); - } + // convert ellipse direction back to swing axis + vSwingAxis.setZ(-y); + vSwingAxis.setY( z); + vSwingAxis.normalize(); + } +} + + + +void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) +{ + btTransform trACur = m_rbA.getCenterOfMassTransform(); + btTransform trBCur = m_rbB.getCenterOfMassTransform(); + btTransform trABCur = trBCur.inverse() * trACur; + btQuaternion qABCur = trABCur.getRotation(); + btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); + btQuaternion qConstraintCur = trConstraintCur.getRotation(); + + btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); + setMotorTargetInConstraintSpace(qConstraint); +} - // solve twist limit - if (m_solveTwistLimit) - { - btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor ); - btScalar impulseMag = amplitude * m_kTwist; - // Clamp the accumulated impulse - btScalar temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accTwistLimitImpulse - temp; +void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) +{ + m_qTarget = q; + + // clamp motor target to within limits + { + btScalar softness = 1.f;//m_limitSoftness; - btVector3 impulse = m_twistAxis * impulseMag; + // split into twist and cone + btVector3 vTwisted = quatRotate(m_qTarget, vTwist); + btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); + btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); - m_rbA.applyTorqueImpulse(impulse); - m_rbB.applyTorqueImpulse(-impulse); + // clamp cone + if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) + { + btScalar swingAngle, swingLimit; btVector3 swingAxis; + computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); + if (fabs(swingAngle) > SIMD_EPSILON) + { + if (swingAngle > swingLimit*softness) + swingAngle = swingLimit*softness; + else if (swingAngle < -swingLimit*softness) + swingAngle = -swingLimit*softness; + qTargetCone = btQuaternion(swingAxis, swingAngle); + } } - - } -} + // clamp twist + if (m_twistSpan >= btScalar(0.05f)) + { + btScalar twistAngle; btVector3 twistAxis; + computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); -void btConeTwistConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; + if (fabs(twistAngle) > SIMD_EPSILON) + { + // eddy todo: limitSoftness used here??? + if (twistAngle > m_twistSpan*softness) + twistAngle = m_twistSpan*softness; + else if (twistAngle < -m_twistSpan*softness) + twistAngle = -m_twistSpan*softness; + qTargetTwist = btQuaternion(twistAxis, twistAngle); + } + } + m_qTarget = qTargetCone * qTargetTwist; + } } + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index f121919c8f9..84ea9e04095 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -20,7 +20,7 @@ Written by: Marcus Hennix #ifndef CONETWISTCONSTRAINT_H #define CONETWISTCONSTRAINT_H -#include "../../LinearMath/btVector3.h" +#include "LinearMath/btVector3.h" #include "btJacobianEntry.h" #include "btTypedConstraint.h" @@ -42,10 +42,14 @@ public: btScalar m_biasFactor; btScalar m_relaxationFactor; + btScalar m_damping; + btScalar m_swingSpan1; btScalar m_swingSpan2; btScalar m_twistSpan; + btScalar m_fixThresh; + btVector3 m_swingAxis; btVector3 m_twistAxis; @@ -56,6 +60,8 @@ public: btScalar m_swingCorrection; btScalar m_twistCorrection; + btScalar m_twistAngle; + btScalar m_accSwingLimitImpulse; btScalar m_accTwistLimitImpulse; @@ -63,6 +69,19 @@ public: bool m_solveTwistLimit; bool m_solveSwingLimit; + bool m_useSolveConstraintObsolete; + + // not yet used... + btScalar m_swingLimitRatio; + btScalar m_twistLimitRatio; + btVector3 m_twistAxisA; + + // motor + bool m_bMotorEnabled; + bool m_bNormalizedMotorStrength; + btQuaternion m_qTarget; + btScalar m_maxMotorImpulse; + btVector3 m_accMotorImpulse; public: @@ -74,7 +93,12 @@ public: virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); @@ -92,7 +116,32 @@ public: m_angularOnly = angularOnly; } - void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) + void setLimit(int limitIndex,btScalar limitValue) + { + switch (limitIndex) + { + case 3: + { + m_twistSpan = limitValue; + break; + } + case 4: + { + m_swingSpan2 = limitValue; + break; + } + case 5: + { + m_swingSpan1 = limitValue; + break; + } + default: + { + } + }; + } + + void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { m_swingSpan1 = _swingSpan1; m_swingSpan2 = _swingSpan2; @@ -121,6 +170,60 @@ public: return m_twistLimitSign; } + void calcAngleInfo(); + void calcAngleInfo2(); + + inline btScalar getSwingSpan1() + { + return m_swingSpan1; + } + inline btScalar getSwingSpan2() + { + return m_swingSpan2; + } + inline btScalar getTwistSpan() + { + return m_twistSpan; + } + inline btScalar getTwistAngle() + { + return m_twistAngle; + } + bool isPastSwingLimit() { return m_solveSwingLimit; } + + + void setDamping(btScalar damping) { m_damping = damping; } + + void enableMotor(bool b) { m_bMotorEnabled = b; } + void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; } + void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; } + + btScalar getFixThresh() { return m_fixThresh; } + void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; } + + // setMotorTarget: + // q: the desired rotation of bodyA wrt bodyB. + // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability) + // note: don't forget to enableMotor() + void setMotorTarget(const btQuaternion &q); + + // same as above, but q is the desired rotation of frameA wrt frameB in constraint space + void setMotorTargetInConstraintSpace(const btQuaternion &q); + + btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const; + + + +protected: + void init(); + + void computeConeLimitInfo(const btQuaternion& qCone, // in + btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs + + void computeTwistLimitInfo(const btQuaternion& qTwist, // in + btScalar& twistAngle, btVector3& vTwistAxis); // all outs + + void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const; }; #endif //CONETWISTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 4d7cd05feb7..012c321fd6d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" -#define ASSERT2 assert +#define ASSERT2 btAssert #define USE_INTERNAL_APPLY_IMPULSE 1 @@ -52,7 +52,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btVector3 vel = vel1 - vel2; - btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), + btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), body2.getCenterOfMassTransform().getBasis().transpose(), rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), body2.getInvInertiaDiagLocal(),body2.getInvMass()); @@ -114,7 +114,7 @@ btScalar resolveSingleCollision( btScalar Kcor = Kerp *Kfps; btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - assert(cpd); + btAssert(cpd); btScalar distance = cpd->m_penetration; btScalar positionalError = Kcor *-distance; btScalar velocityError = cpd->m_restitution - rel_vel;// * damping; @@ -166,7 +166,7 @@ btScalar resolveSingleFriction( btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - assert(cpd); + btAssert(cpd); btScalar combinedFriction = cpd->m_friction; @@ -255,7 +255,7 @@ btScalar resolveSingleFrictionOriginal( btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - assert(cpd); + btAssert(cpd); btScalar combinedFriction = cpd->m_friction; @@ -337,7 +337,7 @@ btScalar resolveSingleCollisionCombined( btScalar Kcor = Kerp *Kfps; btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; - assert(cpd); + btAssert(cpd); btScalar distance = cpd->m_penetration; btScalar positionalError = Kcor *-distance; btScalar velocityError = cpd->m_restitution - rel_vel;// * damping; @@ -425,5 +425,5 @@ btScalar resolveSingleFrictionEmpty( return btScalar(0.); -}; +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index 826e79f78bd..e8871f3860b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef CONTACT_CONSTRAINT_H #define CONTACT_CONSTRAINT_H -//todo: make into a proper class working with the iterative constraint solver +///@todo: make into a proper class working with the iterative constraint solver class btRigidBody; #include "LinearMath/btVector3.h" diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 916d4581f79..e99430c00de 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -21,7 +21,13 @@ enum btSolverMode SOLVER_RANDMIZE_ORDER = 1, SOLVER_FRICTION_SEPARATE = 2, SOLVER_USE_WARMSTARTING = 4, - SOLVER_CACHE_FRIENDLY = 8 + SOLVER_USE_FRICTION_WARMSTARTING = 8, + SOLVER_USE_2_FRICTION_DIRECTIONS = 16, + SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32, + SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64, + SOLVER_CACHE_FRIENDLY = 128, + SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version + SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster. }; struct btContactSolverInfoData @@ -38,12 +44,14 @@ struct btContactSolverInfoData btScalar m_sor; btScalar m_erp;//used as Baumgarte factor btScalar m_erp2;//used in Split Impulse + btScalar m_globalCfm;//constraint force mixing int m_splitImpulse; btScalar m_splitImpulsePenetrationThreshold; btScalar m_linearSlop; btScalar m_warmstartingFactor; int m_solverMode; + int m_restingContactRestitutionThreshold; }; @@ -63,12 +71,14 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.1); - m_sor = btScalar(1.3); + m_globalCfm = btScalar(0.); + m_sor = btScalar(1.); m_splitImpulse = false; m_splitImpulsePenetrationThreshold = -0.02f; m_linearSlop = btScalar(0.0); m_warmstartingFactor=btScalar(0.85); - m_solverMode = SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY | SOLVER_USE_WARMSTARTING; + m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER + m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution } }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 077b326d13a..6cbfe61f700 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -19,18 +19,40 @@ email: projectileman@yahoo.com http://gimpact.sf.net */ - #include "btGeneric6DofConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" #include -static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) }; -static const int kAxisA[] = { 1, 0, 0 }; -static const int kAxisB[] = { 2, 2, 1 }; +#define D6_USE_OBSOLETE_METHOD false +//----------------------------------------------------------------------------- + +btGeneric6DofConstraint::btGeneric6DofConstraint() +:btTypedConstraint(D6_CONSTRAINT_TYPE), +m_useLinearReferenceFrameA(true), +m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) +{ +} + +//----------------------------------------------------------------------------- + +btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) +: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) +, m_frameInA(frameInA) +, m_frameInB(frameInB), +m_useLinearReferenceFrameA(useLinearReferenceFrameA), +m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) +{ + +} +//----------------------------------------------------------------------------- + + #define GENERIC_D6_DISABLE_WARMSTARTING 1 +//----------------------------------------------------------------------------- + btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) { @@ -39,51 +61,48 @@ btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) return mat[i][j]; } +//----------------------------------------------------------------------------- + ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) { -// // rot = cy*cz -cy*sz sy -// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx -// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy -// + // // rot = cy*cz -cy*sz sy + // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + // - if (btGetMatrixElem(mat,2) < btScalar(1.0)) + btScalar fi = btGetMatrixElem(mat,2); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) { - if (btGetMatrixElem(mat,2) > btScalar(-1.0)) - { - xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); - xyz[1] = btAsin(btGetMatrixElem(mat,2)); - xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); - return true; - } - else - { - // WARNING. Not unique. XA - ZA = -atan2(r10,r11) - xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - xyz[1] = -SIMD_HALF_PI; - xyz[2] = btScalar(0.0); - return false; - } + xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); + xyz[1] = btAsin(btGetMatrixElem(mat,2)); + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + return true; } else { - // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - xyz[1] = SIMD_HALF_PI; - xyz[2] = 0.0; - + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = btScalar(0.0); + return false; } - - + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = SIMD_HALF_PI; + xyz[2] = 0.0; + } return false; } - - //////////////////////////// btRotationalLimitMotor //////////////////////////////////// - int btRotationalLimitMotor::testLimitValue(btScalar test_value) { if(m_loLimit>m_hiLimit) @@ -107,212 +126,239 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) m_currentLimit = 0;//Free from violation return 0; - + } +//----------------------------------------------------------------------------- btScalar btRotationalLimitMotor::solveAngularLimits( - btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, - btRigidBody * body0, btRigidBody * body1) + btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, + btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB) { - if (needApplyTorques()==false) return 0.0f; + if (needApplyTorques()==false) return 0.0f; - btScalar target_velocity = m_targetVelocity; - btScalar maxMotorForce = m_maxMotorForce; + btScalar target_velocity = m_targetVelocity; + btScalar maxMotorForce = m_maxMotorForce; //current error correction - if (m_currentLimit!=0) - { - target_velocity = -m_ERP*m_currentLimitError/(timeStep); - maxMotorForce = m_maxLimitForce; - } + if (m_currentLimit!=0) + { + target_velocity = -m_ERP*m_currentLimitError/(timeStep); + maxMotorForce = m_maxLimitForce; + } - maxMotorForce *= timeStep; + maxMotorForce *= timeStep; - // current velocity difference - btVector3 vel_diff = body0->getAngularVelocity(); - if (body1) - { - vel_diff -= body1->getAngularVelocity(); - } + // current velocity difference + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); + btVector3 vel_diff; + vel_diff = angVelA-angVelB; - btScalar rel_vel = axis.dot(vel_diff); + + + btScalar rel_vel = axis.dot(vel_diff); // correction velocity - btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); + btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); - if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) - { - return 0.0f;//no need for applying force - } + if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) + { + return 0.0f;//no need for applying force + } // correction impulse - btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; + btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; // clip correction impulse - btScalar clippedMotorImpulse; + btScalar clippedMotorImpulse; - //todo: should clip against accumulated impulse - if (unclippedMotorImpulse>0.0f) - { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; - } - else - { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; - } + ///@todo: should clip against accumulated impulse + if (unclippedMotorImpulse>0.0f) + { + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; + } + else + { + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; + } // sort with accumulated impulses - btScalar lo = btScalar(-1e30); - btScalar hi = btScalar(1e30); - - btScalar oldaccumImpulse = m_accumulatedImpulse; - btScalar sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); - clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + btScalar oldaccumImpulse = m_accumulatedImpulse; + btScalar sum = oldaccumImpulse + clippedMotorImpulse; + m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + btVector3 motorImp = clippedMotorImpulse * axis; - btVector3 motorImp = clippedMotorImpulse * axis; + //body0->applyTorqueImpulse(motorImp); + //body1->applyTorqueImpulse(-motorImp); + bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); + bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); - body0->applyTorqueImpulse(motorImp); - if (body1) body1->applyTorqueImpulse(-motorImp); - return clippedMotorImpulse; + return clippedMotorImpulse; } //////////////////////////// End btRotationalLimitMotor //////////////////////////////////// + + + //////////////////////////// btTranslationalLimitMotor //////////////////////////////////// -btScalar btTranslationalLimitMotor::solveLinearAxis( - btScalar timeStep, - btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, - int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos) + + +int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value) { + btScalar loLimit = m_lowerLimit[limitIndex]; + btScalar hiLimit = m_upperLimit[limitIndex]; + if(loLimit > hiLimit) + { + m_currentLimit[limitIndex] = 0;//Free from violation + m_currentLimitError[limitIndex] = btScalar(0.f); + return 0; + } -///find relative velocity -// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); -// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); - btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); + if (test_value < loLimit) + { + m_currentLimit[limitIndex] = 2;//low limit violation + m_currentLimitError[limitIndex] = test_value - loLimit; + return 2; + } + else if (test_value> hiLimit) + { + m_currentLimit[limitIndex] = 1;//High limit violation + m_currentLimitError[limitIndex] = test_value - hiLimit; + return 1; + }; - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; + m_currentLimit[limitIndex] = 0;//Free from violation + m_currentLimitError[limitIndex] = btScalar(0.f); + return 0; +} // btTranslationalLimitMotor::testLimitValue() - btScalar rel_vel = axis_normal_on_a.dot(vel); +//----------------------------------------------------------------------------- +btScalar btTranslationalLimitMotor::solveLinearAxis( + btScalar timeStep, + btScalar jacDiagABInv, + btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, + btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, + int limit_index, + const btVector3 & axis_normal_on_a, + const btVector3 & anchorPos) +{ + ///find relative velocity + // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); + // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); + btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); -/// apply displacement correction + btVector3 vel1; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + btVector3 vel2; + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; -//positional error (zeroth order error) - btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); - btScalar lo = btScalar(-1e30); - btScalar hi = btScalar(1e30); + btScalar rel_vel = axis_normal_on_a.dot(vel); - btScalar minLimit = m_lowerLimit[limit_index]; - btScalar maxLimit = m_upperLimit[limit_index]; - //handle the limits - if (minLimit < maxLimit) - { - { - if (depth > maxLimit) - { - depth -= maxLimit; - lo = btScalar(0.); - } - else - { - if (depth < minLimit) - { - depth -= minLimit; - hi = btScalar(0.); - } - else - { - return 0.0f; - } - } - } - } + /// apply displacement correction - btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; + //positional error (zeroth order error) + btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); + btScalar minLimit = m_lowerLimit[limit_index]; + btScalar maxLimit = m_upperLimit[limit_index]; + //handle the limits + if (minLimit < maxLimit) + { + { + if (depth > maxLimit) + { + depth -= maxLimit; + lo = btScalar(0.); + + } + else + { + if (depth < minLimit) + { + depth -= minLimit; + hi = btScalar(0.); + } + else + { + return 0.0f; + } + } + } + } + btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; - btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; - btScalar sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; - btVector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1.applyImpulse( impulse_vector, rel_pos1); - body2.applyImpulse(-impulse_vector, rel_pos2); - return normalImpulse; -} -//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// + btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; + btScalar sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; -btGeneric6DofConstraint::btGeneric6DofConstraint() - :btTypedConstraint(D6_CONSTRAINT_TYPE), - m_useLinearReferenceFrameA(true) -{ -} + btVector3 impulse_vector = axis_normal_on_a * normalImpulse; + //body1.applyImpulse( impulse_vector, rel_pos1); + //body2.applyImpulse(-impulse_vector, rel_pos2); -btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) - , m_frameInA(frameInA) - , m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) -{ + btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); + btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); + bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); -} + return normalImpulse; +} +//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// void btGeneric6DofConstraint::calculateAngleInfo() { btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); - matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); - - - // in euler angle mode we do not actually constrain the angular velocity - // along the axes axis[0] and axis[2] (although we do use axis[1]) : - // - // to get constrain w2-w1 along ...not - // ------ --------------------- ------ - // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] - // d(angle[1])/dt = 0 ax[1] - // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] - // - // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. - // to prove the result for angle[0], write the expression for angle[0] from - // GetInfo1 then take the derivative. to prove this for angle[2] it is - // easier to take the euler rate expression for d(angle[2])/dt with respect - // to the components of w and set that to 0. - + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); @@ -320,34 +366,29 @@ void btGeneric6DofConstraint::calculateAngleInfo() m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); - -// if(m_debugDrawer) -// { -// -// char buff[300]; -// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", -// m_calculatedAxisAngleDiff[0], -// m_calculatedAxisAngleDiff[1], -// m_calculatedAxisAngleDiff[2]); -// m_debugDrawer->reportErrorWarning(buff); -// } + m_calculatedAxis[0].normalize(); + m_calculatedAxis[1].normalize(); + m_calculatedAxis[2].normalize(); } +//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::calculateTransforms() { - m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; - m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; - - calculateAngleInfo(); + m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; + m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; + calculateLinearInfo(); + calculateAngleInfo(); } +//----------------------------------------------------------------------------- void btGeneric6DofConstraint::buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW) + btJacobianEntry & jacLinear,const btVector3 & normalWorld, + const btVector3 & pivotAInW,const btVector3 & pivotBInW) { - new (&jacLinear) btJacobianEntry( + new (&jacLinear) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), @@ -357,13 +398,14 @@ void btGeneric6DofConstraint::buildLinearJacobian( m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); - } +//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::buildAngularJacobian( - btJacobianEntry & jacAngular,const btVector3 & jointAxisW) + btJacobianEntry & jacAngular,const btVector3 & jointAxisW) { - new (&jacAngular) btJacobianEntry(jointAxisW, + new (&jacAngular) btJacobianEntry(jointAxisW, m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getInvInertiaDiagLocal(), @@ -371,142 +413,260 @@ void btGeneric6DofConstraint::buildAngularJacobian( } +//----------------------------------------------------------------------------- + bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { - btScalar angle = m_calculatedAxisAngleDiff[axis_index]; - - //test limits - m_angularLimits[axis_index].testLimitValue(angle); - return m_angularLimits[axis_index].needApplyTorques(); + btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + //test limits + m_angularLimits[axis_index].testLimitValue(angle); + return m_angularLimits[axis_index].needApplyTorques(); } +//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::buildJacobian() { + if (m_useSolveConstraintObsolete) + { - // Clear accumulated impulses for the next simulation step - m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); - int i; - for(i = 0; i < 3; i++) - { - m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); - } - //calculates transform - calculateTransforms(); - -// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); -// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); - calcAnchorPos(); - btVector3 pivotAInW = m_AnchorPos; - btVector3 pivotBInW = m_AnchorPos; - -// not used here -// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); -// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 normalWorld; - //linear part - for (i=0;i<3;i++) - { - if (m_linearLimits.isLimited(i)) - { - if (m_useLinearReferenceFrameA) - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - else - normalWorld = m_calculatedTransformB.getBasis().getColumn(i); + // Clear accumulated impulses for the next simulation step + m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + int i; + for(i = 0; i < 3; i++) + { + m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); + } + //calculates transform + calculateTransforms(); + + // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); + // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); + calcAnchorPos(); + btVector3 pivotAInW = m_AnchorPos; + btVector3 pivotBInW = m_AnchorPos; + + // not used here + // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 normalWorld; + //linear part + for (i=0;i<3;i++) + { + if (m_linearLimits.isLimited(i)) + { + if (m_useLinearReferenceFrameA) + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + else + normalWorld = m_calculatedTransformB.getBasis().getColumn(i); - buildLinearJacobian( - m_jacLinear[i],normalWorld , - pivotAInW,pivotBInW); + buildLinearJacobian( + m_jacLinear[i],normalWorld , + pivotAInW,pivotBInW); - } - } + } + } - // angular part - for (i=0;i<3;i++) - { - //calculates error angle - if (testAngularLimitMotor(i)) - { - normalWorld = this->getAxis(i); - // Create angular atom - buildAngularJacobian(m_jacAng[i],normalWorld); - } - } + // angular part + for (i=0;i<3;i++) + { + //calculates error angle + if (testAngularLimitMotor(i)) + { + normalWorld = this->getAxis(i); + // Create angular atom + buildAngularJacobian(m_jacAng[i],normalWorld); + } + } + + } +} +//----------------------------------------------------------------------------- +void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } else + { + //prepare constraint + calculateTransforms(); + info->m_numConstraintRows = 0; + info->nub = 6; + int i; + //test linear limits + for(i = 0; i < 3; i++) + { + if(m_linearLimits.needApplyForce(i)) + { + info->m_numConstraintRows++; + info->nub--; + } + } + //test angular limits + for (i=0;i<3 ;i++ ) + { + if(testAngularLimitMotor(i)) + { + info->m_numConstraintRows++; + info->nub--; + } + } + } +} + +//----------------------------------------------------------------------------- + +void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + int row = setLinearLimits(info); + setAngularLimits(info, row); } +//----------------------------------------------------------------------------- -void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) +int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) { - m_timeStep = timeStep; + btGeneric6DofConstraint * d6constraint = this; + int row = 0; + //solve linear limits + btRotationalLimitMotor limot; + for (int i=0;i<3 ;i++ ) + { + if(m_linearLimits.needApplyForce(i)) + { // re-use rotational motor code + limot.m_bounce = btScalar(0.f); + limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; + limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; + limot.m_damping = m_linearLimits.m_damping; + limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; + limot.m_ERP = m_linearLimits.m_restitution; + limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; + limot.m_limitSoftness = m_linearLimits.m_limitSoftness; + limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; + limot.m_maxLimitForce = btScalar(0.f); + limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; + limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; + btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); + row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0); + } + } + return row; +} - //calculateTransforms(); +//----------------------------------------------------------------------------- - int i; +int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset) +{ + btGeneric6DofConstraint * d6constraint = this; + int row = row_offset; + //solve angular limits + for (int i=0;i<3 ;i++ ) + { + if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) + { + btVector3 axis = d6constraint->getAxis(i); + row += get_limit_motor_info2( + d6constraint->getRotationalLimitMotor(i), + &m_rbA, + &m_rbB, + info,row,axis,1); + } + } - // linear + return row; +} - btVector3 pointInA = m_calculatedTransformA.getOrigin(); - btVector3 pointInB = m_calculatedTransformB.getOrigin(); +//----------------------------------------------------------------------------- - btScalar jacDiagABInv; - btVector3 linear_axis; - for (i=0;i<3;i++) - { - if (m_linearLimits.isLimited(i)) - { - jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); +void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { - if (m_useLinearReferenceFrameA) - linear_axis = m_calculatedTransformA.getBasis().getColumn(i); - else - linear_axis = m_calculatedTransformB.getBasis().getColumn(i); - m_linearLimits.solveLinearAxis( - m_timeStep, - jacDiagABInv, - m_rbA,pointInA, - m_rbB,pointInB, - i,linear_axis, m_AnchorPos); + m_timeStep = timeStep; - } - } + //calculateTransforms(); - // angular - btVector3 angular_axis; - btScalar angularJacDiagABInv; - for (i=0;i<3;i++) - { - if (m_angularLimits[i].needApplyTorques()) - { + int i; - // get axis - angular_axis = getAxis(i); + // linear - angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); + btVector3 pointInA = m_calculatedTransformA.getOrigin(); + btVector3 pointInB = m_calculatedTransformB.getOrigin(); - m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB); - } - } + btScalar jacDiagABInv; + btVector3 linear_axis; + for (i=0;i<3;i++) + { + if (m_linearLimits.isLimited(i)) + { + jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); + + if (m_useLinearReferenceFrameA) + linear_axis = m_calculatedTransformA.getBasis().getColumn(i); + else + linear_axis = m_calculatedTransformB.getBasis().getColumn(i); + + m_linearLimits.solveLinearAxis( + m_timeStep, + jacDiagABInv, + m_rbA,bodyA,pointInA, + m_rbB,bodyB,pointInB, + i,linear_axis, m_AnchorPos); + + } + } + + // angular + btVector3 angular_axis; + btScalar angularJacDiagABInv; + for (i=0;i<3;i++) + { + if (m_angularLimits[i].needApplyTorques()) + { + + // get axis + angular_axis = getAxis(i); + + angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); + + m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB); + } + } + } } +//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { - (void)timeStep; + (void)timeStep; } +//----------------------------------------------------------------------------- + btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const { - return m_calculatedAxis[axis_index]; + return m_calculatedAxis[axis_index]; } +//----------------------------------------------------------------------------- + btScalar btGeneric6DofConstraint::getAngle(int axis_index) const { - return m_calculatedAxisAngleDiff[axis_index]; + return m_calculatedAxisAngleDiff[axis_index]; } +//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::calcAnchorPos(void) { btScalar imA = m_rbA.getInvMass(); @@ -526,3 +686,144 @@ void btGeneric6DofConstraint::calcAnchorPos(void) return; } // btGeneric6DofConstraint::calcAnchorPos() +//----------------------------------------------------------------------------- + +void btGeneric6DofConstraint::calculateLinearInfo() +{ + m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); + m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; + for(int i = 0; i < 3; i++) + { + m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); + } +} // btGeneric6DofConstraint::calculateLinearInfo() + +//----------------------------------------------------------------------------- + +int btGeneric6DofConstraint::get_limit_motor_info2( + btRotationalLimitMotor * limot, + btRigidBody * body0, btRigidBody * body1, + btConstraintInfo2 *info, int row, btVector3& ax1, int rotational) +{ + int srow = row * info->rowskip; + int powered = limot->m_enableMotor; + int limit = limot->m_currentLimit; + if (powered || limit) + { // if the joint is powered, or has joint limits, add in the extra row + btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; + btScalar *J2 = rotational ? info->m_J2angularAxis : 0; + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + if(rotational) + { + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + } + if((!rotational) && limit) + { + btVector3 ltd; // Linear Torque Decoupling vector + btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition(); + ltd = c.cross(ax1); + info->m_J1angularAxis[srow+0] = ltd[0]; + info->m_J1angularAxis[srow+1] = ltd[1]; + info->m_J1angularAxis[srow+2] = ltd[2]; + + c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition(); + ltd = -c.cross(ax1); + info->m_J2angularAxis[srow+0] = ltd[0]; + info->m_J2angularAxis[srow+1] = ltd[1]; + info->m_J2angularAxis[srow+2] = ltd[2]; + } + // if we're limited low and high simultaneously, the joint motor is + // ineffective + if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; + info->m_constraintError[srow] = btScalar(0.f); + if (powered) + { + info->cfm[srow] = 0.0f; + if(!limit) + { + info->m_constraintError[srow] += limot->m_targetVelocity; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce; + info->m_upperLimit[srow] = limot->m_maxMotorForce; + } + } + if(limit) + { + btScalar k = info->fps * limot->m_ERP; + if(!rotational) + { + info->m_constraintError[srow] += k * limot->m_currentLimitError; + } + else + { + info->m_constraintError[srow] += -k * limot->m_currentLimitError; + } + info->cfm[srow] = 0.0f; + if (limot->m_loLimit == limot->m_hiLimit) + { // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { + if (limit == 1) + { + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + // deal with bounce + if (limot->m_bounce > 0) + { + // calculate joint velocity + btScalar vel; + if (rotational) + { + vel = body0->getAngularVelocity().dot(ax1); + if (body1) + vel -= body1->getAngularVelocity().dot(ax1); + } + else + { + vel = body0->getLinearVelocity().dot(ax1); + if (body1) + vel -= body1->getLinearVelocity().dot(ax1); + } + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if (limit == 1) + { + if (vel < 0) + { + btScalar newc = -limot->m_bounce* vel; + if (newc > info->m_constraintError[srow]) + info->m_constraintError[srow] = newc; + } + } + else + { + if (vel > 0) + { + btScalar newc = -limot->m_bounce * vel; + if (newc < info->m_constraintError[srow]) + info->m_constraintError[srow] = newc; + } + } + } + } + } + return 1; + } + else return 0; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index f0718d2d4a0..0ae161d5bdf 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -30,6 +30,8 @@ http://gimpact.sf.net class btRigidBody; + + //! Rotation Limit structure for generic joints class btRotationalLimitMotor { @@ -92,7 +94,7 @@ public: //! Is limited bool isLimited() { - if(m_loLimit>=m_hiLimit) return false; + if(m_loLimit > m_hiLimit) return false; return true; } @@ -110,8 +112,7 @@ public: int testLimitValue(btScalar test_value); //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); - + btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB); }; @@ -129,6 +130,11 @@ public: btScalar m_damping;//!< Damping for linear limit btScalar m_restitution;//! Bounce parameter for linear limit //!@} + bool m_enableMotor[3]; + btVector3 m_targetVelocity;//!< target motor velocity + btVector3 m_maxMotorForce;//!< max force on motor + btVector3 m_currentLimitError;//! How much is violated this limit + int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit btTranslationalLimitMotor() { @@ -139,6 +145,12 @@ public: m_limitSoftness = 0.7f; m_damping = btScalar(1.0f); m_restitution = btScalar(0.5f); + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = false; + m_targetVelocity[i] = btScalar(0.f); + m_maxMotorForce[i] = btScalar(0.f); + } } btTranslationalLimitMotor(const btTranslationalLimitMotor & other ) @@ -150,6 +162,12 @@ public: m_limitSoftness = other.m_limitSoftness ; m_damping = other.m_damping; m_restitution = other.m_restitution; + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = other.m_enableMotor[i]; + m_targetVelocity[i] = other.m_targetVelocity[i]; + m_maxMotorForce[i] = other.m_maxMotorForce[i]; + } } //! Test limit @@ -163,13 +181,19 @@ public: { return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); } + inline bool needApplyForce(int limitIndex) + { + if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; + return true; + } + int testLimitValue(int limitIndex, btScalar test_value); btScalar solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, + btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, + btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos); @@ -247,6 +271,7 @@ protected: btTransform m_calculatedTransformB; btVector3 m_calculatedAxisAngleDiff; btVector3 m_calculatedAxis[3]; + btVector3 m_calculatedLinearDiff; btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes @@ -262,6 +287,9 @@ protected: } + int setAngularLimits(btConstraintInfo2 *info, int row_offset); + + int setLinearLimits(btConstraintInfo2 *info); void buildLinearJacobian( btJacobianEntry & jacLinear,const btVector3 & normalWorld, @@ -269,6 +297,8 @@ protected: void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW); + // tests linear limits + void calculateLinearInfo(); //! calcs the euler angles between the two bodies. void calculateAngleInfo(); @@ -276,6 +306,10 @@ protected: public: + + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; + btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); btGeneric6DofConstraint(); @@ -330,7 +364,11 @@ public: //! performs Jacobian calculation, and also calculates angle differences and axis virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); @@ -432,6 +470,11 @@ public: virtual void calcAnchorPos(void); // overridable + int get_limit_motor_info2( btRotationalLimitMotor * limot, + btRigidBody * body0, btRigidBody * body1, + btConstraintInfo2 *info, int row, btVector3& ax1, int rotational); + + }; #endif //GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index a0523a8c76b..b6b34305804 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -19,19 +19,33 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" #include "LinearMath/btMinMax.h" #include +#include "btSolverBody.h" + +//----------------------------------------------------------------------------- + +#define HINGE_USE_OBSOLETE_SOLVER false + +//----------------------------------------------------------------------------- btHingeConstraint::btHingeConstraint() : btTypedConstraint (HINGE_CONSTRAINT_TYPE), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(false) { + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, - btVector3& axisInA,btVector3& axisInB) + btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), m_angularOnly(false), - m_enableAngularMotor(false) + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), + m_useReferenceFrameA(useReferenceFrameA) { m_rbAFrame.getOrigin() = pivotInA; @@ -60,9 +74,9 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = pivotInB; - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() ); + m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), + rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), + rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); @@ -71,32 +85,28 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; - + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- -btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) +btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA) +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(useReferenceFrameA) { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace - btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); - btScalar projection = rbAxisA1.dot(axisInA); - if (projection > SIMD_EPSILON) - rbAxisA1 = rbAxisA1*projection - axisInA; - else - rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - - btVector3 rbAxisA2 = axisInA.cross(rbAxisA1); + btVector3 rbAxisA1, rbAxisA2; + btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); - - btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA; + btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); @@ -115,19 +125,19 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA, m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, - const btTransform& rbAFrame, const btTransform& rbBFrame) + const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), m_angularOnly(false), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(useReferenceFrameA) { - // flip axis - m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); - //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); @@ -135,22 +145,20 @@ m_enableAngularMotor(false) m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- - -btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame) +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), m_angularOnly(false), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), +m_useReferenceFrameA(useReferenceFrameA) { ///not providing rigidbody B means implicitly using worldspace for body B - // flip axis - m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); - m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); //start with free @@ -160,33 +168,38 @@ m_enableAngularMotor(false) m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; + m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } +//----------------------------------------------------------------------------- + void btHingeConstraint::buildJacobian() { - m_appliedImpulse = btScalar(0.); - - if (!m_angularOnly) + if (m_useSolveConstraintObsolete) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; + m_appliedImpulse = btScalar(0.); - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) + if (!m_angularOnly) { - normal[0] = relPos.normalized(); - } - else - { - normal[0].setValue(btScalar(1.0),0,0); - } + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 relPos = pivotBInW - pivotAInW; + + btVector3 normal[3]; + if (relPos.length2() > SIMD_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0].setValue(btScalar(1.0),0,0); + } - btPlaneSpace1(normal[0], normal[1], normal[2]); + btPlaneSpace1(normal[0], normal[1], normal[2]); - for (int i=0;i<3;i++) - { - new (&m_jac[i]) btJacobianEntry( + for (int i=0;i<3;i++) + { + new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), @@ -196,214 +209,458 @@ void btHingeConstraint::buildJacobian() m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); + } } - } - //calculate two perpendicular jointAxis, orthogonal to hingeAxis - //these two jointAxis require equal angular velocities for both bodies + //calculate two perpendicular jointAxis, orthogonal to hingeAxis + //these two jointAxis require equal angular velocities for both bodies - //this is unused for now, it's a todo - btVector3 jointAxis0local; - btVector3 jointAxis1local; - - btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); - - getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; - btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; - btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + //this is unused for now, it's a todo + btVector3 jointAxis0local; + btVector3 jointAxis1local; - new (&m_jacAng[0]) btJacobianEntry(jointAxis0, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[1]) btJacobianEntry(jointAxis1, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); + getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; + btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; + btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + + new (&m_jacAng[0]) btJacobianEntry(jointAxis0, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + new (&m_jacAng[1]) btJacobianEntry(jointAxis1, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + // clear accumulator + m_accLimitImpulse = btScalar(0.); + + // test angular limit + testLimit(); + + //Compute K = J*W*J' for hinge axis + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + + getRigidBodyB().computeAngularImpulseDenominator(axisA)); - // Compute limit information - btScalar hingeAngle = getHingeAngle(); + } +} - //set bias, sign, clear accumulator - m_correction = btScalar(0.); - m_limitSign = btScalar(0.); - m_solveLimit = false; - m_accLimitImpulse = btScalar(0.); +//----------------------------------------------------------------------------- -// if (m_lowerLimit < m_upperLimit) - if (m_lowerLimit <= m_upperLimit) +void btHingeConstraint::getInfo1(btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) { -// if (hingeAngle <= m_lowerLimit*m_limitSoftness) - if (hingeAngle <= m_lowerLimit) - { - m_correction = (m_lowerLimit - hingeAngle); - m_limitSign = 1.0f; - m_solveLimit = true; - } -// else if (hingeAngle >= m_upperLimit*m_limitSoftness) - else if (hingeAngle >= m_upperLimit) + info->m_numConstraintRows = 0; + info->nub = 0; + } + else + { + info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular + info->nub = 1; + //prepare constraint + testLimit(); + if(getSolveLimit() || getEnableAngularMotor()) { - m_correction = m_upperLimit - hingeAngle; - m_limitSign = -1.0f; - m_solveLimit = true; + info->m_numConstraintRows++; // limit 3rd anguar as well + info->nub--; } } +} // btHingeConstraint::getInfo1 () - //Compute K = J*W*J' for hinge axis - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + - getRigidBodyB().computeAngularImpulseDenominator(axisA)); +//----------------------------------------------------------------------------- +void btHingeConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + int i, s = info->rowskip; + // transforms in world space + btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; + btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame; + // pivot point + btVector3 pivotAInW = trA.getOrigin(); + btVector3 pivotBInW = trB.getOrigin(); + // linear (all fixed) + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[s + 1] = 1; + info->m_J1linearAxis[2 * s + 2] = 1; + btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + // linear RHS + btScalar k = info->fps * info->erp; + for(i = 0; i < 3; i++) + { + info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]); + } + // make rotations around X and Y equal + // the hinge axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the hinge axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the hinge axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + // get hinge axis (Z) + btVector3 ax1 = trA.getBasis().getColumn(2); + // get 2 orthos to hinge axis (X, Y) + btVector3 p = trA.getBasis().getColumn(0); + btVector3 q = trA.getBasis().getColumn(1); + // set the two hinge angular rows + int s3 = 3 * info->rowskip; + int s4 = 4 * info->rowskip; + + info->m_J1angularAxis[s3 + 0] = p[0]; + info->m_J1angularAxis[s3 + 1] = p[1]; + info->m_J1angularAxis[s3 + 2] = p[2]; + info->m_J1angularAxis[s4 + 0] = q[0]; + info->m_J1angularAxis[s4 + 1] = q[1]; + info->m_J1angularAxis[s4 + 2] = q[2]; + + info->m_J2angularAxis[s3 + 0] = -p[0]; + info->m_J2angularAxis[s3 + 1] = -p[1]; + info->m_J2angularAxis[s3 + 2] = -p[2]; + info->m_J2angularAxis[s4 + 0] = -q[0]; + info->m_J2angularAxis[s4 + 1] = -q[1]; + info->m_J2angularAxis[s4 + 2] = -q[2]; + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1,ax2 are the unit length hinge axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + btVector3 ax2 = trB.getBasis().getColumn(2); + btVector3 u = ax1.cross(ax2); + info->m_constraintError[s3] = k * u.dot(p); + info->m_constraintError[s4] = k * u.dot(q); + // check angular limits + int nrow = 4; // last filled row + int srow; + btScalar limit_err = btScalar(0.0); + int limit = 0; + if(getSolveLimit()) + { + limit_err = m_correction * m_referenceSign; + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } + // if the hinge has joint limits or motor, add in the extra row + int powered = 0; + if(getEnableAngularMotor()) + { + powered = 1; + } + if(limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->m_J1angularAxis[srow+0] = ax1[0]; + info->m_J1angularAxis[srow+1] = ax1[1]; + info->m_J1angularAxis[srow+2] = ax1[2]; + + info->m_J2angularAxis[srow+0] = -ax1[0]; + info->m_J2angularAxis[srow+1] = -ax1[1]; + info->m_J2angularAxis[srow+2] = -ax1[2]; + + btScalar lostop = getLowerLimit(); + btScalar histop = getUpperLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + info->m_constraintError[srow] = btScalar(0.0f); + if(powered) + { + info->cfm[srow] = btScalar(0.0); + btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp); + info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; + info->m_lowerLimit[srow] = - m_maxMotorImpulse; + info->m_upperLimit[srow] = m_maxMotorImpulse; + } + if(limit) + { + k = info->fps * info->erp; + info->m_constraintError[srow] += k * limit_err; + info->cfm[srow] = btScalar(0.0); + if(lostop == histop) + { + // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else if(limit == 1) + { // low limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { // high limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) + btScalar bounce = m_relaxationFactor; + if(bounce > btScalar(0.0)) + { + btScalar vel = m_rbA.getAngularVelocity().dot(ax1); + vel -= m_rbB.getAngularVelocity().dot(ax1); + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if(limit == 1) + { // low limit + if(vel < 0) + { + btScalar newc = -bounce * vel; + if(newc > info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + else + { // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + } + info->m_constraintError[srow] *= m_biasFactor; + } // if(limit) + } // if angular limit or powered } -void btHingeConstraint::solveConstraint(btScalar timeStep) +//----------------------------------------------------------------------------- + +void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + if (m_useSolveConstraintObsolete) + { + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btScalar tau = btScalar(0.3); + btScalar tau = btScalar(0.3); - //linear part - if (!m_angularOnly) - { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - for (int i=0;i<3;i++) - { - const btVector3& normal = m_jac[i].m_linearJointAxis; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btScalar rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + //linear part + if (!m_angularOnly) + { + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1,vel2; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; + + for (int i=0;i<3;i++) + { + const btVector3& normal = m_jac[i].m_linearJointAxis; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + + btScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + btVector3 impulse_vector = normal * impulse; + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); + btVector3 ftorqueAxis2 = rel_pos2.cross(normal); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); + } } - } - - { - ///solve angular part + + { + ///solve angular part - // get axes in world space - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); + // get axes in world space + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); - const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); - const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); - btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); - btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); + btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); + btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); - btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; - btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; - btVector3 velrelOrthog = angAorthog-angBorthog; - { - //solve orthogonal angular velocity correction - btScalar relaxation = btScalar(1.); - btScalar len = velrelOrthog.length(); - if (len > btScalar(0.00001)) + btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; + btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; + btVector3 velrelOrthog = angAorthog-angBorthog; { - btVector3 normal = velrelOrthog.normalized(); - btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + - getRigidBodyB().computeAngularImpulseDenominator(normal); - // scale for mass and relaxation - //todo: expose this 0.9 factor to developer - velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; - } + - //solve angular positional correction - btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); - btScalar len2 = angularError.length(); - if (len2>btScalar(0.00001)) - { - btVector3 normal2 = angularError.normalized(); - btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + - getRigidBodyB().computeAngularImpulseDenominator(normal2); - angularError *= (btScalar(1.)/denom2) * relaxation; - } + //solve orthogonal angular velocity correction + btScalar relaxation = btScalar(1.); + btScalar len = velrelOrthog.length(); + if (len > btScalar(0.00001)) + { + btVector3 normal = velrelOrthog.normalized(); + btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + getRigidBodyB().computeAngularImpulseDenominator(normal); + // scale for mass and relaxation + //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; - m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); - m_rbB.applyTorqueImpulse(velrelOrthog-angularError); + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); - // solve limit - if (m_solveLimit) - { - btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; + } - btScalar impulseMag = amplitude * m_kHinge; + //solve angular positional correction + btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) + { + btVector3 normal2 = angularError.normalized(); + btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + getRigidBodyB().computeAngularImpulseDenominator(normal2); + //angularError *= (btScalar(1.)/denom2) * relaxation; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); - // Clamp the accumulated impulse - btScalar temp = m_accLimitImpulse; - m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); - impulseMag = m_accLimitImpulse - temp; + } + + - btVector3 impulse = axisA * impulseMag * m_limitSign; - m_rbA.applyTorqueImpulse(impulse); - m_rbB.applyTorqueImpulse(-impulse); - } - } - //apply motor - if (m_enableAngularMotor) - { - //todo: add limits too - btVector3 angularLimit(0,0,0); + // solve limit + if (m_solveLimit) + { + btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; + + btScalar impulseMag = amplitude * m_kHinge; + + // Clamp the accumulated impulse + btScalar temp = m_accLimitImpulse; + m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); + impulseMag = m_accLimitImpulse - temp; + + + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); + + } + } - btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; - btScalar projRelVel = velrel.dot(axisA); + //apply motor + if (m_enableAngularMotor) + { + //todo: add limits too + btVector3 angularLimit(0,0,0); - btScalar desiredMotorVel = m_motorTargetVelocity; - btScalar motor_relvel = desiredMotorVel - projRelVel; + btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; + btScalar projRelVel = velrel.dot(axisA); - btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; - //todo: should clip against accumulated impulse - btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; - clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; - btVector3 motorImp = clippedMotorImpulse * axisA; + btScalar desiredMotorVel = m_motorTargetVelocity; + btScalar motor_relvel = desiredMotorVel - projRelVel; - m_rbA.applyTorqueImpulse(motorImp+angularLimit); - m_rbB.applyTorqueImpulse(-motorImp-angularLimit); + btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; + //todo: should clip against accumulated impulse + btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; + btVector3 motorImp = clippedMotorImpulse * axisA; + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); + + } } } } +//----------------------------------------------------------------------------- + void btHingeConstraint::updateRHS(btScalar timeStep) { (void)timeStep; } +//----------------------------------------------------------------------------- + btScalar btHingeConstraint::getHingeAngle() { const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1); - - return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) ); + btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); + return m_referenceSign * angle; } +//----------------------------------------------------------------------------- + +void btHingeConstraint::testLimit() +{ + // Compute limit information + m_hingeAngle = getHingeAngle(); + m_correction = btScalar(0.); + m_limitSign = btScalar(0.); + m_solveLimit = false; + if (m_lowerLimit <= m_upperLimit) + { + if (m_hingeAngle <= m_lowerLimit) + { + m_correction = (m_lowerLimit - m_hingeAngle); + m_limitSign = 1.0f; + m_solveLimit = true; + } + else if (m_hingeAngle >= m_upperLimit) + { + m_correction = m_upperLimit - m_hingeAngle; + m_limitSign = -1.0f; + m_solveLimit = true; + } + } + return; +} // btHingeConstraint::testLimit() + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 4fa9972f6d8..0af655f4409 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -53,27 +53,35 @@ public: btScalar m_correction; btScalar m_accLimitImpulse; + btScalar m_hingeAngle; + btScalar m_referenceSign; bool m_angularOnly; bool m_enableAngularMotor; bool m_solveLimit; + bool m_useSolveConstraintObsolete; + bool m_useReferenceFrameA; public: - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB); + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false); - btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA); + btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false); - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame); + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false); - btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame); + btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false); btHingeConstraint(); virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); @@ -86,6 +94,16 @@ public: return m_rbB; } + btRigidBody& getRigidBodyA() + { + return m_rbA; + } + + btRigidBody& getRigidBodyB() + { + return m_rbB; + } + void setAngularOnly(bool angularOnly) { m_angularOnly = angularOnly; @@ -122,6 +140,8 @@ public: btScalar getHingeAngle(); + void testLimit(); + const btTransform& getAFrame() { return m_rbAFrame; }; const btTransform& getBFrame() { return m_rbBFrame; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index 2b69ad90438..1da749517e8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -21,33 +21,38 @@ subject to the following restrictions: btPoint2PointConstraint::btPoint2PointConstraint() -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE), +m_useSolveConstraintObsolete(false) { } btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), +m_useSolveConstraintObsolete(false) { } btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), +m_useSolveConstraintObsolete(false) { } void btPoint2PointConstraint::buildJacobian() { - m_appliedImpulse = btScalar(0.); + ///we need it for both methods + { + m_appliedImpulse = btScalar(0.); - btVector3 normal(0,0,0); + btVector3 normal(0,0,0); - for (int i=0;i<3;i++) - { - normal[i] = 1; - new (&m_jac[i]) btJacobianEntry( + for (int i=0;i<3;i++) + { + normal[i] = 1; + new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), @@ -58,64 +63,162 @@ void btPoint2PointConstraint::buildJacobian() m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); normal[i] = 0; + } } } -void btPoint2PointConstraint::solveConstraint(btScalar timeStep) -{ - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; +void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } else + { + info->m_numConstraintRows = 3; + info->nub = 3; + } +} - btVector3 normal(0,0,0); +void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + + //retrieve matrices + btTransform body0_trans; + body0_trans = m_rbA.getCenterOfMassTransform(); + btTransform body1_trans; + body1_trans = m_rbB.getCenterOfMassTransform(); + + // anchor points in global coordinates with respect to body PORs. + + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; + + btVector3 a1 = body0_trans.getBasis()*getPivotInA(); + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + /*info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[s+1] = -1; + info->m_J2linearAxis[2*s+2] = -1; + */ + btVector3 a2 = body1_trans.getBasis()*getPivotInB(); + + { + btVector3 a2n = -a2; + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + -// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); -// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - for (int i=0;i<3;i++) - { - normal[i] = 1; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - //this jacobian entry could be re-used for all iterations - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - btScalar rel_vel; - rel_vel = normal.dot(vel); + // set right hand side + btScalar k = info->fps * info->erp; + int j; - /* - //velocity error (first order error) - btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - */ - - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - - btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } - btScalar impulseClamp = m_setting.m_impulseClamp; - if (impulseClamp > 0) + btScalar impulseClamp = m_setting.m_impulseClamp;// + for (j=0; j<3; j++) + { + if (m_setting.m_impulseClamp > 0) { - if (impulse < -impulseClamp) - impulse = -impulseClamp; - if (impulse > impulseClamp) - impulse = impulseClamp; + info->m_lowerLimit[j*info->rowskip] = -impulseClamp; + info->m_upperLimit[j*info->rowskip] = impulseClamp; } + } + +} + - m_appliedImpulse+=impulse; - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); +void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + + btVector3 normal(0,0,0); - normal[i] = 0; + + // btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + // btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + //this jacobian entry could be re-used for all iterations + + btVector3 vel1,vel2; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; + + btScalar rel_vel; + rel_vel = normal.dot(vel); + + /* + //velocity error (first order error) + btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + */ + + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + + btScalar deltaImpulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; + + btScalar impulseClamp = m_setting.m_impulseClamp; + + const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse; + if (sum < -impulseClamp) + { + deltaImpulse = -impulseClamp-m_appliedImpulse; + m_appliedImpulse = -impulseClamp; + } + else if (sum > impulseClamp) + { + deltaImpulse = impulseClamp-m_appliedImpulse; + m_appliedImpulse = impulseClamp; + } + else + { + m_appliedImpulse = sum; + } + + + btVector3 impulse_vector = normal * deltaImpulse; + + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); + btVector3 ftorqueAxis2 = rel_pos2.cross(normal); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse); + + + normal[i] = 0; + } } } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index c9d5968530c..e2b865cd484 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -50,6 +50,9 @@ public: public: + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; + btConstraintSetting m_setting; btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB); @@ -60,8 +63,12 @@ public: virtual void buildJacobian(); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + - virtual void solveConstraint(btScalar timeStep); + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 41e336c9d17..6928bdb966b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -15,7 +15,6 @@ subject to the following restrictions: //#define COMPUTE_IMPULSE_DENOM 1 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. -//#define FORCE_REFESH_CONTACT_MANIFOLDS 1 #include "btSequentialImpulseConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -32,441 +31,268 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "btSolverBody.h" #include "btSolverConstraint.h" - - #include "LinearMath/btAlignedObjectArray.h" +#include //for memset +btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() +:m_btSeed2(0) +{ -int totalCpd = 0; +} -int gTotalContactPoints = 0; +btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() +{ +} -struct btOrderIndex +#ifdef USE_SIMD +#include +#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) +static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 ) { - int m_manifoldIndex; - int m_pointIndex; -}; + __m128 result = _mm_mul_ps( vec0, vec1); + return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) ); +} +#endif//USE_SIMD +// Project Gauss Seidel or the equivalent Sequential Impulse +void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ +#ifdef USE_SIMD + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); + btSimdScalar resultLowerLess,resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass)); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass)); + __m128 impulseMagnitude = deltaImpulse; + body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); +#else + resolveSingleConstraintRowGeneric(body1,body2,c); +#endif +} +// Project Gauss Seidel or the equivalent Sequential Impulse + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); + const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); -#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384 -static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS]; + const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + if (body1.m_invMass) + body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); + if (body2.m_invMass) + body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); +} -unsigned long btSequentialImpulseConstraintSolver::btRand2() + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { - m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; - return m_btSeed2; +#ifdef USE_SIMD + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); + btSimdScalar resultLowerLess,resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass)); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass)); + __m128 impulseMagnitude = deltaImpulse; + body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); +#else + resolveSingleConstraintRowLowerLimit(body1,body2,c); +#endif } - - -//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) -int btSequentialImpulseConstraintSolver::btRandInt2 (int n) +// Project Gauss Seidel or the equivalent Sequential Impulse + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { - // seems good; xor-fold and modulus - const unsigned long un = static_cast(n); - unsigned long r = btRand2(); - - // note: probably more aggressive than it needs to be -- might be - // able to get away without one or two of the innermost branches. - if (un <= 0x00010000UL) { - r ^= (r >> 16); - if (un <= 0x00000100UL) { - r ^= (r >> 8); - if (un <= 0x00000010UL) { - r ^= (r >> 4); - if (un <= 0x00000004UL) { - r ^= (r >> 2); - if (un <= 0x00000002UL) { - r ^= (r >> 1); - } - } - } - } - } - - return (int) (r % un); + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); + const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); + + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else + { + c.m_appliedImpulse = sum; + } + if (body1.m_invMass) + body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); + if (body2.m_invMass) + body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); } - -bool MyContactDestroyedCallback(void* userPersistentData); -bool MyContactDestroyedCallback(void* userPersistentData) +unsigned long btSequentialImpulseConstraintSolver::btRand2() { - assert (userPersistentData); - btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData; - btAlignedFree(cpd); - totalCpd--; - //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData); - return true; + m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; + return m_btSeed2; } -btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_btSeed2(0) +//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) +int btSequentialImpulseConstraintSolver::btRandInt2 (int n) { - gContactDestroyedCallback = &MyContactDestroyedCallback; - - //initialize default friction/contact funcs - int i,j; - for (i=0;i(n); + unsigned long r = btRand2(); + + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + if (un <= 0x00010000UL) { + r ^= (r >> 16); + if (un <= 0x00000100UL) { + r ^= (r >> 8); + if (un <= 0x00000010UL) { + r ^= (r >> 4); + if (un <= 0x00000004UL) { + r ^= (r >> 2); + if (un <= 0x00000002UL) { + r ^= (r >> 1); + } + } + } } + } + + return (int) (r % un); } -btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() -{ -} -void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); -void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) +void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) { - btRigidBody* rb = btRigidBody::upcast(collisionObject); + btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; + + solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); + solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + if (rb) { - solverBody->m_angularVelocity = rb->getAngularVelocity() ; - solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin(); - solverBody->m_friction = collisionObject->getFriction(); solverBody->m_invMass = rb->getInvMass(); - solverBody->m_linearVelocity = rb->getLinearVelocity(); solverBody->m_originalBody = rb; solverBody->m_angularFactor = rb->getAngularFactor(); } else { - solverBody->m_angularVelocity.setValue(0,0,0); - solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin(); - solverBody->m_friction = collisionObject->getFriction(); solverBody->m_invMass = 0.f; - solverBody->m_linearVelocity.setValue(0,0,0); solverBody->m_originalBody = 0; solverBody->m_angularFactor = 1.f; } - - solverBody->m_pushVelocity.setValue(0.f,0.f,0.f); - solverBody->m_turnVelocity.setValue(0.f,0.f,0.f); } int gNumSplitImpulseRecoveries = 0; -btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); -btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) +btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) { btScalar rest = restitution * -rel_vel; return rest; } -void resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo); - -//SIMD_FORCE_INLINE -void resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo) -{ - (void)solverInfo; - - if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold) - { - - gNumSplitImpulseRecoveries++; - btScalar normalImpulse; - - // Optimized version of projected relative velocity, use precomputed cross products with normal - // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); - // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); - // btVector3 vel = vel1 - vel2; - // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); - - btScalar rel_vel; - btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity) - + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity); - btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity) - + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity); - - rel_vel = vel1Dotn-vel2Dotn; - - - btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep; - // btScalar positionalError = contactConstraint.m_penetration; - - btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping; - - btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv; - normalImpulse = penetrationImpulse+velocityImpulse; - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse; - btScalar sum = oldNormalImpulse + normalImpulse; - contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum; - - normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse; - - body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse); - - body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse); - - } - -} - - -//velocity + friction -//response between two dynamic objects with friction - -btScalar resolveSingleCollisionCombinedCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo); -//SIMD_FORCE_INLINE -btScalar resolveSingleCollisionCombinedCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo) +void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); +void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) { - (void)solverInfo; - - btScalar normalImpulse; - + if (colObj && colObj->hasAnisotropicFriction()) { - - - // Optimized version of projected relative velocity, use precomputed cross products with normal - // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); - // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); - // btVector3 vel = vel1 - vel2; - // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); - - btScalar rel_vel; - btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) - + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); - btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) - + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); - - rel_vel = vel1Dotn-vel2Dotn; - - btScalar positionalError = 0.f; - if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold)) - { - positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep; - } - - btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping; - - btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv; - normalImpulse = penetrationImpulse+velocityImpulse; - - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse; - btScalar sum = oldNormalImpulse + normalImpulse; - contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; - - normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse; - - body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass, - contactConstraint.m_angularComponentA,normalImpulse); - - body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass, - contactConstraint.m_angularComponentB,-normalImpulse); + // transform to local coordinates + btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); + const btVector3& friction_scaling = colObj->getAnisotropicFriction(); + //apply anisotropic friction + loc_lateral *= friction_scaling; + // ... and transform it back to global coordinates + frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; } - - return normalImpulse; -} - -//#define NO_FRICTION_TANGENTIALS 1 -#ifndef NO_FRICTION_TANGENTIALS - -btScalar resolveSingleFrictionCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo, - btScalar appliedNormalImpulse); - -//SIMD_FORCE_INLINE -btScalar resolveSingleFrictionCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo, - btScalar appliedNormalImpulse) -{ - (void)solverInfo; - - - const btScalar combinedFriction = contactConstraint.m_friction; - - const btScalar limit = appliedNormalImpulse * combinedFriction; - - if (appliedNormalImpulse>btScalar(0.)) - //friction - { - - btScalar j1; - { - - btScalar rel_vel; - const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) - + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); - const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) - + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); - rel_vel = vel1Dotn-vel2Dotn; - - // calculate j that moves us to zero relative velocity - j1 = -rel_vel * contactConstraint.m_jacDiagABInv; -#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1 -#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE - btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse; - contactConstraint.m_appliedImpulse = oldTangentImpulse + j1; - - if (limit < contactConstraint.m_appliedImpulse) - { - contactConstraint.m_appliedImpulse = limit; - } else - { - if (contactConstraint.m_appliedImpulse < -limit) - contactConstraint.m_appliedImpulse = -limit; - } - j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse; -#else - if (limit < j1) - { - j1 = limit; - } else - { - if (j1 < -limit) - j1 = -limit; - } - -#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE - - //GEN_set_min(contactConstraint.m_appliedImpulse, limit); - //GEN_set_max(contactConstraint.m_appliedImpulse, -limit); - - - - } - - body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1); - - body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1); - - } - return 0.f; } -#else -//velocity + friction -//response between two dynamic objects with friction -btScalar resolveSingleFrictionCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo) +btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation) { - btVector3 vel1; - btVector3 vel2; - btScalar normalImpulse(0.f); - - { - const btVector3& normal = contactConstraint.m_contactNormal; - if (contactConstraint.m_penetration < 0.f) - return 0.f; - - - body1.getVelocityInLocalPoint(contactConstraint.m_relpos1CrossNormal,vel1); - body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; - rel_vel = normal.dot(vel); - - btVector3 lat_vel = vel - normal * rel_vel; - btScalar lat_rel_vel = lat_vel.length2(); - - btScalar combinedFriction = contactConstraint.m_friction; - const btVector3& rel_pos1 = contactConstraint.m_rel_posA; - const btVector3& rel_pos2 = contactConstraint.m_rel_posB; - - - if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON) - { - lat_rel_vel = btSqrt(lat_rel_vel); - - lat_vel /= lat_rel_vel; - btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel); - btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel); - btScalar friction_impulse = lat_rel_vel / - (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); - btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction; - - btSetMin(friction_impulse, normal_impulse); - btSetMin(friction_impulse, -normal_impulse); - body1.internalApplyImpulse(lat_vel * -friction_impulse, rel_pos1); - body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); - } - } - - return normalImpulse; -} - -#endif //NO_FRICTION_TANGENTIALS - - - - - -void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation) -{ btRigidBody* body0=btRigidBody::upcast(colObj0); btRigidBody* body1=btRigidBody::upcast(colObj1); - btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand(); + btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand(); + memset(&solverConstraint,0xff,sizeof(btSolverConstraint)); solverConstraint.m_contactNormal = normalAxis; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; solverConstraint.m_frictionIndex = frictionIndex; solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_originalContactPoint = 0; - solverConstraint.m_appliedImpulse = btScalar(0.); - solverConstraint.m_appliedPushImpulse = 0.f; - solverConstraint.m_penetration = 0.f; + solverConstraint.m_appliedImpulse = 0.f; + // solverConstraint.m_appliedPushImpulse = 0.f; + { btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0); + solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); } { - btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); + btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0); + solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); } #ifdef COMPUTE_IMPULSE_DENOM @@ -483,7 +309,7 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& } if (body1) { - vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = body1->getInvMass() + normalAxis.dot(vec); } @@ -492,362 +318,328 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; - -} +#ifdef _USE_JACOBIAN + solverConstraint.m_jac = btJacobianEntry ( + rel_pos1,rel_pos2,solverConstraint.m_contactNormal, + body0->getInvInertiaDiagLocal(), + body0->getInvMass(), + body1->getInvInertiaDiagLocal(), + body1->getInvMass()); +#endif //_USE_JACOBIAN -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) -{ - if (colObj && colObj->hasAnisotropicFriction()) { - // transform to local coordinates - btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); - const btVector3& friction_scaling = colObj->getAnisotropicFriction(); - //apply anisotropic friction - loc_lateral *= friction_scaling; - // ... and transform it back to global coordinates - frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; - } -} + btScalar rel_vel; + btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0)); + btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0)); + + rel_vel = vel1Dotn+vel2Dotn; + btScalar positionalError = 0.f; + btSimdScalar velocityError = - rel_vel; + btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + return solverConstraint; +} -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) { - BT_PROFILE("solveGroupCacheFriendlySetup"); - (void)stackAlloc; - (void)debugDrawer; - + int solverBodyIdA = -1; - if (!(numConstraints + numManifolds)) + if (body.getCompanionId() >= 0) { -// printf("empty\n"); - return 0.f; + //body has already been converted + solverBodyIdA = body.getCompanionId(); + } else + { + btRigidBody* rb = btRigidBody::upcast(&body); + if (rb && rb->getInvMass()) + { + solverBodyIdA = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,&body); + body.setCompanionId(solverBodyIdA); + } else + { + return 0;//assume first one is a fixed solver body + } } - btPersistentManifold* manifold = 0; - btCollisionObject* colObj0=0,*colObj1=0; + return solverBodyIdA; +} +#include - //btRigidBody* rb0=0,*rb1=0; -#ifdef FORCE_REFESH_CONTACT_MANIFOLDS +void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +{ + btCollisionObject* colObj0=0,*colObj1=0; - BEGIN_PROFILE("refreshManifolds"); + colObj0 = (btCollisionObject*)manifold->getBody0(); + colObj1 = (btCollisionObject*)manifold->getBody1(); - int i; - - + int solverBodyIdA=-1; + int solverBodyIdB=-1; - for (i=0;igetNumContacts()) { - manifold = manifoldPtr[i]; - rb1 = (btRigidBody*)manifold->getBody1(); - rb0 = (btRigidBody*)manifold->getBody0(); - - manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); - + solverBodyIdA = getOrInitSolverBody(*colObj0); + solverBodyIdB = getOrInitSolverBody(*colObj1); } - END_PROFILE("refreshManifolds"); -#endif //FORCE_REFESH_CONTACT_MANIFOLDS - - + btVector3 rel_pos1; + btVector3 rel_pos2; + btScalar relaxation; + for (int j=0;jgetNumContacts();j++) + { + btManifoldPoint& cp = manifold->getContactPoint(j); - //int sizeofSB = sizeof(btSolverBody); - //int sizeofSC = sizeof(btSolverConstraint); + if (cp.getDistance() <= manifold->getContactProcessingThreshold()) + { + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); - //if (1) - { - //if m_stackAlloc, try to pack bodies/constraints to speed up solving -// btBlock* sablock; -// sablock = stackAlloc->beginBlock(); + rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - // int memsize = 16; -// unsigned char* stackMemory = stackAlloc->allocate(memsize); - - //todo: use stack allocator for this temp memory -// int minReservation = numManifolds*2; + relaxation = 1.f; + btScalar rel_vel; + btVector3 vel; - //m_tmpSolverBodyPool.reserve(minReservation); + int frictionIndex = m_tmpSolverContactConstraintPool.size(); - //don't convert all bodies, only the one we need so solver the constraints -/* - { - for (int i=0;igetIslandTag() >= 0)) - { - btAssert(rb->getCompanionId() < 0); - int solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb); - rb->setCompanionId(solverBodyId); - } - } - } -*/ - - //m_tmpSolverConstraintPool.reserve(minReservation); - //m_tmpSolverFrictionConstraintPool.reserve(minReservation); + btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand(); + btRigidBody* rb0 = btRigidBody::upcast(colObj0); + btRigidBody* rb1 = btRigidBody::upcast(colObj1); - { - int i; + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; - for (i=0;igetBody0(); - colObj1 = (btCollisionObject*)manifold->getBody1(); - - int solverBodyIdA=-1; - int solverBodyIdB=-1; + solverConstraint.m_originalContactPoint = &cp; - if (manifold->getNumContacts()) + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); { - - - - if (colObj0->getIslandTag() >= 0) - { - if (colObj0->getCompanionId() >= 0) - { - //body has already been converted - solverBodyIdA = colObj0->getCompanionId(); - } else - { - solverBodyIdA = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj0); - colObj0->setCompanionId(solverBodyIdA); - } - } else +#ifdef COMPUTE_IMPULSE_DENOM + btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); +#else + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + if (rb0) { - //create a static body - solverBodyIdA = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj0); + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); } - - if (colObj1->getIslandTag() >= 0) - { - if (colObj1->getCompanionId() >= 0) - { - solverBodyIdB = colObj1->getCompanionId(); - } else - { - solverBodyIdB = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj1); - colObj1->setCompanionId(solverBodyIdB); - } - } else + if (rb1) { - //create a static body - solverBodyIdB = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj1); + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); } +#endif //COMPUTE_IMPULSE_DENOM + + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; } - btVector3 rel_pos1; - btVector3 rel_pos2; - btScalar relaxation; + solverConstraint.m_contactNormal = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB); - for (int j=0;jgetNumContacts();j++) - { - - btManifoldPoint& cp = manifold->getContactPoint(j); - - if (cp.getDistance() <= btScalar(0.)) - { - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); + btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - - relaxation = 1.f; - btScalar rel_vel; - btVector3 vel; + vel = vel1 - vel2; - int frictionIndex = m_tmpSolverConstraintPool.size(); + rel_vel = cp.m_normalWorldOnB.dot(vel); - { - btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand(); - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); + btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; - solverConstraint.m_originalContactPoint = &cp; + solverConstraint.m_friction = cp.m_combinedFriction; - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0); - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0); - { -#ifdef COMPUTE_IMPULSE_DENOM - btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); -#else - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } - if (rb1) - { - vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } -#endif //COMPUTE_IMPULSE_DENOM - - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - } + btScalar restitution = 0.f; + + if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold) + { + restitution = 0.f; + } else + { + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + }; + } - solverConstraint.m_contactNormal = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB); + ///warm starting (or zero if disabled) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); + } else + { + solverConstraint.m_appliedImpulse = 0.f; + } + + // solverConstraint.m_appliedPushImpulse = 0.f; + + { + btScalar rel_vel; + btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0)); + btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0)); + + rel_vel = vel1Dotn+vel2Dotn; + + btScalar positionalError = 0.f; + positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; + btScalar velocityError = restitution - rel_vel;// * damping; + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } - btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - - vel = vel1 - vel2; - - rel_vel = cp.m_normalWorldOnB.dot(vel); - - solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.)); - //solverConstraint.m_penetration = cp.getDistance(); - - solverConstraint.m_friction = cp.m_combinedFriction; - solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (solverConstraint.m_restitution <= btScalar(0.)) - { - solverConstraint.m_restitution = 0.f; - }; - - btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep; + /////setup the friction constraints - - if (solverConstraint.m_restitution > penVel) + + if (1) + { + solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + { + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) + { + cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - solverConstraint.m_penetration = btScalar(0.); + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize();//?? + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } - - - - ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); - } else + cp.m_lateralFrictionInitialized = true; + } else + { + //re-calculate friction direction every frame, todo: check if this is really needed + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); + + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - solverConstraint.m_appliedImpulse = 0.f; + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } + cp.m_lateralFrictionInitialized = true; + } + + } else + { + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } - solverConstraint.m_appliedPushImpulse = 0.f; - - solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size(); - if (!cp.m_lateralFrictionInitialized) + if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) + { + { + btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - - //scale anisotropic friction - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - - - if (lat_rel_vel > SIMD_EPSILON)//0.0f) - { - cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize(); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } else - { - //re-calculate friction direction every frame, todo: check if this is really needed - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } - cp.m_lateralFrictionInitialized = true; - + frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); } else { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + frictionConstraint1.m_appliedImpulse = 0.f; } + } + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { - btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; - if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); - if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); - } else - { - frictionConstraint1.m_appliedImpulse = 0.f; - } - } + frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); + } else { - btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; - if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); - if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); - } else - { - frictionConstraint2.m_appliedImpulse = 0.f; - } + frictionConstraint2.m_appliedImpulse = 0.f; } } - - + } else + { + btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + frictionConstraint1.m_appliedImpulse = 0.f; + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + frictionConstraint2.m_appliedImpulse = 0.f; + } } } } + + } } - - btContactSolverInfo info = infoGlobal; +} + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +{ + BT_PROFILE("solveGroupCacheFriendlySetup"); + (void)stackAlloc; + (void)debugDrawer; + + + if (!(numConstraints + numManifolds)) + { + // printf("empty\n"); + return 0.f; + } + + if (1) { int j; for (j=0;jbuildJacobian(); } } - - - int numConstraintPool = m_tmpSolverConstraintPool.size(); - int numFrictionPool = m_tmpSolverFrictionConstraintPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody,0); + + //btRigidBody* rb0=0,*rb1=0; + + //if (1) + { + { + + int totalNumRows = 0; + int i; + //calculate the total number of contraint rows + for (i=0;igetInfo1(&info1); + totalNumRows += info1.m_numConstraintRows; + } + m_tmpSolverNonContactConstraintPool.resize(totalNumRows); + + btTypedConstraint::btConstraintInfo1 info1; + info1.m_numConstraintRows = 0; + + + ///setup the btSolverConstraints + int currentRow = 0; + + for (i=0;igetInfo1(&info1); + if (info1.m_numConstraintRows) + { + btAssert(currentRowgetRigidBodyA(); + btRigidBody& rbB = constraint->getRigidBodyB(); + + int solverBodyIdA = getOrInitSolverBody(rbA); + int solverBodyIdB = getOrInitSolverBody(rbB); + + btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + + int j; + for ( j=0;jm_deltaLinearVelocity.setValue(0.f,0.f,0.f); + bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); + bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + + + + btTypedConstraint::btConstraintInfo2 info2; + info2.fps = 1.f/infoGlobal.m_timeStep; + info2.erp = infoGlobal.m_erp; + info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; + info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxis = 0; + info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this + ///the size of btSolverConstraint needs be a multiple of btScalar + btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); + info2.m_constraintError = ¤tConstraintRow->m_rhs; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + constraints[i]->getInfo2(&info2); + + ///finalize the constraint setup + for ( j=0;jgetRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); + } + { + const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; + solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); + } + + { + btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); + btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; + btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? + btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; + + btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal); + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal); + sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + + solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + btScalar rel_vel; + btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); + btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); + + rel_vel = vel1Dotn+vel2Dotn; + + btScalar restitution = 0.f; + btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 + btScalar velocityError = restitution - rel_vel;// * damping; + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_appliedImpulse = 0.f; + + } + } + } + } + } + + { + int i; + btPersistentManifold* manifold = 0; + btCollisionObject* colObj0=0,*colObj1=0; + + + for (i=0;igetRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + ///solve all joint constraints, using SIMD, if available + for (j=0;jgetRigidBodyA().getCompanionId()].writebackVelocity(); + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); } - if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) + + for (j=0;jgetRigidBodyB().getCompanionId()].writebackVelocity(); + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); } - constraint->solveConstraint(infoGlobal.m_timeStep); - - if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + ///solve all contact constraints using SIMD, if available + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (j=0;jgetRigidBodyA().getCompanionId()].readVelocity(); + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } - if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) + ///solve all friction constraints, using SIMD, if available + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (j=0;jgetRigidBodyB().getCompanionId()].readVelocity(); - } + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - } + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + } else { - int numPoolConstraints = m_tmpSolverConstraintPool.size(); - for (j=0;jgetRigidBodyA()); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], - m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal, - totalImpulse); + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); } - } - - - } - - if (infoGlobal.m_splitImpulse) - { - - for ( iteration = 0;iterationbtScalar(0)) { - const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]]; + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], - m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal); + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } } } - } - } + } + } return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) + +/// btSequentialImpulseConstraintSolver Sequentially applies impulses +btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) { + + + + BT_PROFILE("solveGroup"); + //we only implement SOLVER_CACHE_FRIENDLY now + //you need to provide at least some bodies + btAssert(bodies); + btAssert(numBodies); + int i; solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - int numPoolConstraints = m_tmpSolverConstraintPool.size(); + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int j; + for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; - pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) + { + pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + } //do a callback here? - } if (infoGlobal.m_splitImpulse) @@ -1030,418 +997,26 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio } else { for ( i=0;igetNumContacts();p++) - { - gOrder[totalPoints].m_manifoldIndex = j; - gOrder[totalPoints].m_pointIndex = p; - totalPoints++; - } - } - } - - { - int j; - for (j=0;jbuildJacobian(); + m_tmpSolverBodyPool[i].writebackVelocity(); } } - - - //should traverse the contacts random order... - int iteration; - - { - for ( iteration = 0;iterationsolveConstraint(info.m_timeStep); - } - - for (j=0;jgetBody0(), - (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); - } - - for (j=0;jgetBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); - } - - } - } - - - return btScalar(0.); -} - - - - - - - -void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer) -{ - - (void)debugDrawer; - - btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); - btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); - - - //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop - { -#ifdef FORCE_REFESH_CONTACT_MANIFOLDS - manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform()); -#endif //FORCE_REFESH_CONTACT_MANIFOLDS - int numpoints = manifoldPtr->getNumContacts(); - - gTotalContactPoints += numpoints; - - - for (int i=0;igetContactPoint(i); - if (cp.getDistance() <= btScalar(0.)) - { - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition(); - - - //this jacobian entry is re-used for all iterations - btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(), - body1->getCenterOfMassTransform().getBasis().transpose(), - rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(), - body1->getInvInertiaDiagLocal(),body1->getInvMass()); - - - btScalar jacDiagAB = jac.getDiagonal(); - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - if (cpd) - { - //might be invalid - cpd->m_persistentLifeTime++; - if (cpd->m_persistentLifeTime != cp.getLifeTime()) - { - //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); - new (cpd) btConstraintPersistentData; - cpd->m_persistentLifeTime = cp.getLifeTime(); - - } else - { - //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); - - } - } else - { - - //todo: should this be in a pool? - void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16); - cpd = new (mem)btConstraintPersistentData; - assert(cpd); - - totalCpd ++; - //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); - cp.m_userPersistentData = cpd; - cpd->m_persistentLifeTime = cp.getLifeTime(); - //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); - - } - assert(cpd); - - cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB; - - //Dependent on Rigidbody A and B types, fetch the contact/friction response func - //perhaps do a similar thing for friction/restutution combiner funcs... - - cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType]; - cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType]; - - btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; - rel_vel = cp.m_normalWorldOnB.dot(vel); - - btScalar combinedRestitution = cp.m_combinedRestitution; - - cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations); - cpd->m_friction = cp.m_combinedFriction; - cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); - if (cpd->m_restitution <= btScalar(0.)) - { - cpd->m_restitution = btScalar(0.0); - - }; - - //restitution and penetration work in same direction so - //rel_vel - - btScalar penVel = -cpd->m_penetration/info.m_timeStep; - - if (cpd->m_restitution > penVel) - { - cpd->m_penetration = btScalar(0.); - } - - - btScalar relaxation = info.m_damping; - if (info.m_solverMode & SOLVER_USE_WARMSTARTING) - { - cpd->m_appliedImpulse *= relaxation; - } else - { - cpd->m_appliedImpulse =btScalar(0.); - } - - //for friction - cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse; - - //re-calculate friction direction every frame, todo: check if this is really needed - btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1); - - -#define NO_FRICTION_WARMSTART 1 - - #ifdef NO_FRICTION_WARMSTART - cpd->m_accumulatedTangentImpulse0 = btScalar(0.); - cpd->m_accumulatedTangentImpulse1 = btScalar(0.); - #endif //NO_FRICTION_WARMSTART - btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); - btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); - btScalar denom = relaxation/(denom0+denom1); - cpd->m_jacDiagABInvTangent0 = denom; - - - denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1); - denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1); - denom = relaxation/(denom0+denom1); - cpd->m_jacDiagABInvTangent1 = denom; - - btVector3 totalImpulse = - #ifndef NO_FRICTION_WARMSTART - cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+ - cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+ - #endif //NO_FRICTION_WARMSTART - cp.m_normalWorldOnB*cpd->m_appliedImpulse; - - - - /// - { - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0; - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1; - } - { - btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0); - cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0; - } - { - btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1); - cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1; - } - { - btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0); - cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0; - } - { - btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1); - cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1; - } - - /// - - - - //apply previous frames impulse on both bodies - body0->applyImpulse(totalImpulse, rel_pos1); - body1->applyImpulse(-totalImpulse, rel_pos2); - } - - } - } -} - - -btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) -{ - btScalar maxImpulse = btScalar(0.); - - { - - - { - if (cp.getDistance() <= btScalar(0.)) - { - - - - { - - //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - btScalar impulse = resolveSingleCollisionCombined( - *body0,*body1, - cp, - info); - - if (maxImpulse < impulse) - maxImpulse = impulse; - - } - } - } - } - return maxImpulse; -} - - - -btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) -{ - - btScalar maxImpulse = btScalar(0.); - - { - - - { - if (cp.getDistance() <= btScalar(0.)) - { - - - - { - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - btScalar impulse = cpd->m_contactSolverFunc( - *body0,*body1, - cp, - info); - - if (maxImpulse < impulse) - maxImpulse = impulse; + m_tmpSolverBodyPool.resize(0); + m_tmpSolverContactConstraintPool.resize(0); + m_tmpSolverNonContactConstraintPool.resize(0); + m_tmpSolverContactFrictionConstraintPool.resize(0); - } - } - } - } - return maxImpulse; + return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) -{ - (void)debugDrawer; - (void)iter; - { - - { - - if (cp.getDistance() <= btScalar(0.)) - { - btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - cpd->m_frictionSolverFunc( - *body0,*body1, - cp, - info); - - } - } - - - } - return btScalar(0.); -} void btSequentialImpulseConstraintSolver::reset() diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 7143bc41991..467e37bb91f 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -23,67 +23,62 @@ class btIDebugDraw; #include "btSolverConstraint.h" -/// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses -/// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com -/// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) -/// Applies impulses for combined restitution and penetration recovery and to simulate friction + +///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses +///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com +///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) +///Applies impulses for combined restitution and penetration recovery and to simulate friction class btSequentialImpulseConstraintSolver : public btConstraintSolver { +protected: btAlignedObjectArray m_tmpSolverBodyPool; - btAlignedObjectArray m_tmpSolverConstraintPool; - btAlignedObjectArray m_tmpSolverFrictionConstraintPool; + btConstraintArray m_tmpSolverContactConstraintPool; + btConstraintArray m_tmpSolverNonContactConstraintPool; + btConstraintArray m_tmpSolverContactFrictionConstraintPool; btAlignedObjectArray m_orderTmpConstraintPool; btAlignedObjectArray m_orderFrictionConstraintPool; - -protected: - btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); - void addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation); - - ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; - ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; - + btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction unsigned long m_btSeed2; -public: + void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); + btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); - - btSequentialImpulseConstraintSolver(); + void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody - ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType - void setContactSolverFunc(ContactSolverFunc func,int type0,int type1) - { - m_contactDispatch[type0][type1] = func; - } + void resolveSplitPenetrationImpulseCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + const btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo); + + //internal method + int getOrInitSolverBody(btCollisionObject& body); + + void resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + + void resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); - ///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody - ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType - void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1) - { - m_frictionDispatch[type0][type1] = func; - } + void resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + + void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + +public: - virtual ~btSequentialImpulseConstraintSolver(); - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); + btSequentialImpulseConstraintSolver(); + virtual ~btSequentialImpulseConstraintSolver(); - virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); + btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - + btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); ///clear internal cached data and reset random seed virtual void reset(); - - btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - - unsigned long btRand2(); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 4128f504bf1..50d06960379 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -68,7 +68,9 @@ void btSliderConstraint::initParams() btSliderConstraint::btSliderConstraint() :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), - m_useLinearReferenceFrameA(true) + m_useLinearReferenceFrameA(true), + m_useSolveConstraintObsolete(false) +// m_useSolveConstraintObsolete(true) { initParams(); } // btSliderConstraint::btSliderConstraint() @@ -79,7 +81,9 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB) , m_frameInA(frameInA) , m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) + m_useLinearReferenceFrameA(useLinearReferenceFrameA), + m_useSolveConstraintObsolete(false) +// m_useSolveConstraintObsolete(true) { initParams(); } // btSliderConstraint::btSliderConstraint() @@ -88,6 +92,10 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const void btSliderConstraint::buildJacobian() { + if (!m_useSolveConstraintObsolete) + { + return; + } if(m_useLinearReferenceFrameA) { buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB); @@ -155,27 +163,372 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co //----------------------------------------------------------------------------- -void btSliderConstraint::solveConstraint(btScalar timeStep) +void btSliderConstraint::getInfo1(btConstraintInfo1* info) { - m_timeStep = timeStep; - if(m_useLinearReferenceFrameA) + if (m_useSolveConstraintObsolete) { - solveConstraintInt(m_rbA, m_rbB); + info->m_numConstraintRows = 0; + info->nub = 0; } else { - solveConstraintInt(m_rbB, m_rbA); + info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular + info->nub = 2; + //prepare constraint + calculateTransforms(); + testLinLimits(); + if(getSolveLinLimit() || getPoweredLinMotor()) + { + info->m_numConstraintRows++; // limit 3rd linear as well + info->nub--; + } + testAngLimits(); + if(getSolveAngLimit() || getPoweredAngMotor()) + { + info->m_numConstraintRows++; // limit 3rd angular as well + info->nub--; + } + } +} // btSliderConstraint::getInfo1() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::getInfo2(btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + int i, s = info->rowskip; + const btTransform& trA = getCalculatedTransformA(); + const btTransform& trB = getCalculatedTransformB(); + btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); + // make rotations around Y and Z equal + // the slider axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the slider axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the slider axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + // get slider axis (X) + btVector3 ax1 = trA.getBasis().getColumn(0); + // get 2 orthos to slider axis (Y, Z) + btVector3 p = trA.getBasis().getColumn(1); + btVector3 q = trA.getBasis().getColumn(2); + // set the two slider rows + info->m_J1angularAxis[0] = p[0]; + info->m_J1angularAxis[1] = p[1]; + info->m_J1angularAxis[2] = p[2]; + info->m_J1angularAxis[s+0] = q[0]; + info->m_J1angularAxis[s+1] = q[1]; + info->m_J1angularAxis[s+2] = q[2]; + + info->m_J2angularAxis[0] = -p[0]; + info->m_J2angularAxis[1] = -p[1]; + info->m_J2angularAxis[2] = -p[2]; + info->m_J2angularAxis[s+0] = -q[0]; + info->m_J2angularAxis[s+1] = -q[1]; + info->m_J2angularAxis[s+2] = -q[2]; + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the slider back into alignment. + // if ax1,ax2 are the unit length slider axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if "theta" is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); + btVector3 ax2 = trB.getBasis().getColumn(0); + btVector3 u = ax1.cross(ax2); + info->m_constraintError[0] = k * u.dot(p); + info->m_constraintError[s] = k * u.dot(q); + // pull out pos and R for both bodies. also get the connection + // vector c = pos2-pos1. + // next two rows. we want: vel2 = vel1 + w1 x c ... but this would + // result in three equations, so we project along the planespace vectors + // so that sliding along the slider axis is disregarded. for symmetry we + // also consider rotation around center of mass of two bodies (factA and factB). + btTransform bodyA_trans = m_rbA.getCenterOfMassTransform(); + btTransform bodyB_trans = m_rbB.getCenterOfMassTransform(); + int s2 = 2 * s, s3 = 3 * s; + btVector3 c; + btScalar miA = m_rbA.getInvMass(); + btScalar miB = m_rbB.getInvMass(); + btScalar miS = miA + miB; + btScalar factA, factB; + if(miS > btScalar(0.f)) + { + factA = miB / miS; + } + else + { + factA = btScalar(0.5f); + } + if(factA > 0.99f) factA = 0.99f; + if(factA < 0.01f) factA = 0.01f; + factB = btScalar(1.0f) - factA; + c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); + btVector3 tmp = c.cross(p); + for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; + tmp = c.cross(q); + for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; + + for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; + for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; + // compute two elements of right hand side. we want to align the offset + // point (in body 2's frame) with the center of body 1. + btVector3 ofs; // offset point in global coordinates + ofs = trB.getOrigin() - trA.getOrigin(); + k = info->fps * info->erp * getSoftnessOrthoLin(); + info->m_constraintError[s2] = k * p.dot(ofs); + info->m_constraintError[s3] = k * q.dot(ofs); + int nrow = 3; // last filled row + int srow; + // check linear limits linear + btScalar limit_err = btScalar(0.0); + int limit = 0; + if(getSolveLinLimit()) + { + limit_err = getLinDepth() * signFact; + limit = (limit_err > btScalar(0.0)) ? 2 : 1; + } + int powered = 0; + if(getPoweredLinMotor()) + { + powered = 1; + } + // if the slider has joint limits or motor, add in the extra row + if (limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->m_J1linearAxis[srow+0] = ax1[0]; + info->m_J1linearAxis[srow+1] = ax1[1]; + info->m_J1linearAxis[srow+2] = ax1[2]; + // linear torque decoupling step: + // + // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies + // do not create a torque couple. in other words, the points that the + // constraint force is applied at must lie along the same ax1 axis. + // a torque couple will result in limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the center of mass of the two bodies + btVector3 ltd; // Linear Torque Decoupling vector (a torque) +// c = btScalar(0.5) * c; + ltd = c.cross(ax1); + info->m_J1angularAxis[srow+0] = factA*ltd[0]; + info->m_J1angularAxis[srow+1] = factA*ltd[1]; + info->m_J1angularAxis[srow+2] = factA*ltd[2]; + info->m_J2angularAxis[srow+0] = factB*ltd[0]; + info->m_J2angularAxis[srow+1] = factB*ltd[1]; + info->m_J2angularAxis[srow+2] = factB*ltd[2]; + // right-hand part + btScalar lostop = getLowerLinLimit(); + btScalar histop = getUpperLinLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + info->m_constraintError[srow] = 0.; + info->m_lowerLimit[srow] = 0.; + info->m_upperLimit[srow] = 0.; + if(powered) + { + info->cfm[nrow] = btScalar(0.0); + btScalar tag_vel = getTargetLinMotorVelocity(); + btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp); +// info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity(); + info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); + info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps; + info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps; + } + if(limit) + { + k = info->fps * info->erp; + info->m_constraintError[srow] += k * limit_err; + info->cfm[srow] = btScalar(0.0); // stop_cfm; + if(lostop == histop) + { // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else if(limit == 1) + { // low limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + else + { // high limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) + btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); + if(bounce > btScalar(0.0)) + { + btScalar vel = m_rbA.getLinearVelocity().dot(ax1); + vel -= m_rbB.getLinearVelocity().dot(ax1); + vel *= signFact; + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if(limit == 1) + { // low limit + if(vel < 0) + { + btScalar newc = -bounce * vel; + if (newc > info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + else + { // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + } + info->m_constraintError[srow] *= getSoftnessLimLin(); + } // if(limit) + } // if linear limit + // check angular limits + limit_err = btScalar(0.0); + limit = 0; + if(getSolveAngLimit()) + { + limit_err = getAngDepth(); + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } + // if the slider has joint limits, add in the extra row + powered = 0; + if(getPoweredAngMotor()) + { + powered = 1; + } + if(limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->m_J1angularAxis[srow+0] = ax1[0]; + info->m_J1angularAxis[srow+1] = ax1[1]; + info->m_J1angularAxis[srow+2] = ax1[2]; + + info->m_J2angularAxis[srow+0] = -ax1[0]; + info->m_J2angularAxis[srow+1] = -ax1[1]; + info->m_J2angularAxis[srow+2] = -ax1[2]; + + btScalar lostop = getLowerAngLimit(); + btScalar histop = getUpperAngLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + if(powered) + { + info->cfm[srow] = btScalar(0.0); + btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp); + info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); + info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; + info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps; + } + if(limit) + { + k = info->fps * info->erp; + info->m_constraintError[srow] += k * limit_err; + info->cfm[srow] = btScalar(0.0); // stop_cfm; + if(lostop == histop) + { + // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else if(limit == 1) + { // low limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { // high limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) + btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng()); + if(bounce > btScalar(0.0)) + { + btScalar vel = m_rbA.getAngularVelocity().dot(ax1); + vel -= m_rbB.getAngularVelocity().dot(ax1); + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if(limit == 1) + { // low limit + if(vel < 0) + { + btScalar newc = -bounce * vel; + if(newc > info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + else + { // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + } + info->m_constraintError[srow] *= getSoftnessLimAng(); + } // if(limit) + } // if angular limit or powered +} // btSliderConstraint::getInfo2() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { + m_timeStep = timeStep; + if(m_useLinearReferenceFrameA) + { + solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB); + } + else + { + solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); + } } } // btSliderConstraint::solveConstraint() //----------------------------------------------------------------------------- -void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) +void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) { int i; // linear - btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA); - btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB); + btVector3 velA; + bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA); + btVector3 velB; + bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB); btVector3 vel = velA - velB; for(i = 0; i < 3; i++) { @@ -190,8 +543,18 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) // calcutate and apply impulse btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; btVector3 impulse_vector = normal * normalImpulse; - rbA.applyImpulse( impulse_vector, m_relPosA); - rbB.applyImpulse(-impulse_vector, m_relPosB); + + //rbA.applyImpulse( impulse_vector, m_relPosA); + //rbB.applyImpulse(-impulse_vector, m_relPosB); + { + btVector3 ftorqueAxis1 = m_relPosA.cross(normal); + btVector3 ftorqueAxis2 = m_relPosB.cross(normal); + bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + } + + + if(m_poweredLinMotor && (!i)) { // apply linear motor if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) @@ -217,8 +580,18 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; - rbA.applyImpulse( impulse_vector, m_relPosA); - rbB.applyImpulse(-impulse_vector, m_relPosB); + //rbA.applyImpulse( impulse_vector, m_relPosA); + //rbB.applyImpulse(-impulse_vector, m_relPosB); + + { + btVector3 ftorqueAxis1 = m_relPosA.cross(normal); + btVector3 ftorqueAxis2 = m_relPosB.cross(normal); + bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + } + + + } } } @@ -227,8 +600,10 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); - const btVector3& angVelA = rbA.getAngularVelocity(); - const btVector3& angVelB = rbB.getAngularVelocity(); + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); @@ -238,24 +613,38 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) btVector3 velrelOrthog = angAorthog-angBorthog; //solve orthogonal angular velocity correction btScalar len = velrelOrthog.length(); + btScalar orthorImpulseMag = 0.f; + if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); - velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; } //solve angular positional correction btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); + btVector3 angularAxis = angularError; + btScalar angularImpulseMag = 0; + btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); - angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + angularError *= angularImpulseMag; } // apply impulse - rbA.applyTorqueImpulse(-velrelOrthog+angularError); - rbB.applyTorqueImpulse(velrelOrthog-angularError); + //rbA.applyTorqueImpulse(-velrelOrthog+angularError); + //rbB.applyTorqueImpulse(velrelOrthog-angularError); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag); + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag); + + btScalar impulseMag; //solve angular limits if(m_solveAngLim) @@ -269,8 +658,14 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) impulseMag *= m_kAngle * m_softnessDirAng; } btVector3 impulse = axisA * impulseMag; - rbA.applyTorqueImpulse(impulse); - rbB.applyTorqueImpulse(-impulse); + //rbA.applyTorqueImpulse(impulse); + //rbB.applyTorqueImpulse(-impulse); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag); + + + //apply angular motor if(m_poweredAngMotor) { @@ -301,8 +696,11 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) m_accumulatedAngMotorImpulse = new_acc; // apply clamped impulse btVector3 motorImp = angImpulse * axisA; - m_rbA.applyTorqueImpulse(motorImp); - m_rbB.applyTorqueImpulse(-motorImp); + //rbA.applyTorqueImpulse(motorImp); + //rbB.applyTorqueImpulse(-motorImp); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); } } } // btSliderConstraint::solveConstraint() @@ -312,7 +710,7 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) //----------------------------------------------------------------------------- void btSliderConstraint::calculateTransforms(void){ - if(m_useLinearReferenceFrameA) + if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) { m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; @@ -325,7 +723,14 @@ void btSliderConstraint::calculateTransforms(void){ m_realPivotAInW = m_calculatedTransformA.getOrigin(); m_realPivotBInW = m_calculatedTransformB.getOrigin(); m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - m_delta = m_realPivotBInW - m_realPivotAInW; + if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) + { + m_delta = m_realPivotBInW - m_realPivotAInW; + } + else + { + m_delta = m_realPivotAInW - m_realPivotBInW; + } m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; btVector3 normalWorld; int i; @@ -367,7 +772,6 @@ void btSliderConstraint::testLinLimits(void) } // btSliderConstraint::testLinLimits() //----------------------------------------------------------------------------- - void btSliderConstraint::testAngLimits(void) { @@ -379,6 +783,7 @@ void btSliderConstraint::testAngLimits(void) const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + m_angPos = rot; if(rot < m_lowerAngLimit) { m_angDepth = rot - m_lowerAngLimit; @@ -391,12 +796,9 @@ void btSliderConstraint::testAngLimits(void) } } } // btSliderConstraint::testAngLimits() - //----------------------------------------------------------------------------- - - btVector3 btSliderConstraint::getAncorInA(void) { btVector3 ancorInA; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 580dfa1178d..70fbce5d9b2 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -46,6 +46,8 @@ class btRigidBody; class btSliderConstraint : public btTypedConstraint { protected: + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; btTransform m_frameInA; btTransform m_frameInB; // use frameA fo define limits, if true @@ -104,6 +106,7 @@ protected: btVector3 m_relPosB; btScalar m_linPos; + btScalar m_angPos; btScalar m_angDepth; btScalar m_kAngle; @@ -126,7 +129,13 @@ public: btSliderConstraint(); // overrides virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + + // access const btRigidBody& getRigidBodyA() const { return m_rbA; } const btRigidBody& getRigidBodyB() const { return m_rbB; } @@ -194,6 +203,7 @@ public: void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } btScalar getLinearPos() { return m_linPos; } + // access for ODE solver bool getSolveLinLimit() { return m_solveLinLim; } @@ -202,10 +212,11 @@ public: btScalar getAngDepth() { return m_angDepth; } // internal void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); - void solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB); + void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB); // shared code used by ODE solver void calculateTransforms(void); void testLinLimits(void); + void testLinLimits2(btConstraintInfo2* info); void testAngLimits(void); // access for PE Solver btVector3 getAncorInA(void); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h index b3f0c9d7444..98c9876ae46 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -23,86 +23,152 @@ class btRigidBody; #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btTransformUtil.h" +///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision +#ifdef BT_USE_SSE +#define USE_SIMD 1 +#endif // -///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. + +#ifdef USE_SIMD + +struct btSimdScalar +{ + SIMD_FORCE_INLINE btSimdScalar() + { + + } + + SIMD_FORCE_INLINE btSimdScalar(float fl) + :m_vec128 (_mm_set1_ps(fl)) + { + } + + SIMD_FORCE_INLINE btSimdScalar(__m128 v128) + :m_vec128(v128) + { + } + union + { + __m128 m_vec128; + float m_floats[4]; + int m_ints[4]; + btScalar m_unusedPadding; + }; + SIMD_FORCE_INLINE __m128 get128() + { + return m_vec128; + } + + SIMD_FORCE_INLINE const __m128 get128() const + { + return m_vec128; + } + + SIMD_FORCE_INLINE void set128(__m128 v128) + { + m_vec128 = v128; + } + + SIMD_FORCE_INLINE operator __m128() + { + return m_vec128; + } + SIMD_FORCE_INLINE operator const __m128() const + { + return m_vec128; + } + + SIMD_FORCE_INLINE operator float() const + { + return m_floats[0]; + } + +}; + +///@brief Return the elementwise product of two btSimdScalar +SIMD_FORCE_INLINE btSimdScalar +operator*(const btSimdScalar& v1, const btSimdScalar& v2) +{ + return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); +} + +///@brief Return the elementwise product of two btSimdScalar +SIMD_FORCE_INLINE btSimdScalar +operator+(const btSimdScalar& v1, const btSimdScalar& v2) +{ + return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128())); +} + + +#else +#define btSimdScalar btScalar +#endif + +///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. ATTRIBUTE_ALIGNED16 (struct) btSolverBody { BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 m_angularVelocity; - float m_angularFactor; - float m_invMass; - float m_friction; + btVector3 m_deltaLinearVelocity; + btVector3 m_deltaAngularVelocity; + btScalar m_angularFactor; + btScalar m_invMass; + btScalar m_friction; btRigidBody* m_originalBody; - btVector3 m_linearVelocity; - btVector3 m_centerOfMassPosition; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; - + //btVector3 m_turnVelocity; + - SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const { - velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos); + if (m_originalBody) + velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); + else + velocity.setValue(0,0,0); } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const { - if (m_invMass) - { - m_linearVelocity += linearComponent*impulseMagnitude; - m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } + if (m_originalBody) + angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; + else + angVel.setValue(0,0,0); } - - SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) { - if (m_invMass) + //if (m_invMass) { - m_pushVelocity += linearComponent*impulseMagnitude; - m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + m_deltaLinearVelocity += linearComponent*impulseMagnitude; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); } } +/* + void writebackVelocity() { if (m_invMass) { - m_originalBody->setLinearVelocity(m_linearVelocity); - m_originalBody->setAngularVelocity(m_angularVelocity); + m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); + m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); //m_originalBody->setCompanionId(-1); } } - + */ - void writebackVelocity(btScalar timeStep) + void writebackVelocity(btScalar timeStep=0) { if (m_invMass) { - m_originalBody->setLinearVelocity(m_linearVelocity); - m_originalBody->setAngularVelocity(m_angularVelocity); - - //correct the position/orientation based on push/turn recovery - btTransform newTransform; - btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); - m_originalBody->setWorldTransform(newTransform); - + m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity); + m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); //m_originalBody->setCompanionId(-1); } } - - void readVelocity() - { - if (m_invMass) - { - m_linearVelocity = m_originalBody->getLinearVelocity(); - m_angularVelocity = m_originalBody->getAngularVelocity(); - } - } - diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 2c71360c5b9..867d62a301d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -19,40 +19,64 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" +#include "btJacobianEntry.h" //#define NO_FRICTION_TANGENTIALS 1 +#include "btSolverBody.h" + ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal; - - btVector3 m_relpos2CrossNormal; - btVector3 m_angularComponentA; + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal; - btVector3 m_angularComponentB; + btVector3 m_relpos2CrossNormal; + //btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal - mutable btScalar m_appliedPushImpulse; + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + mutable btSimdScalar m_appliedPushImpulse; + mutable btSimdScalar m_appliedImpulse; - mutable btScalar m_appliedImpulse; - int m_solverBodyIdA; - int m_solverBodyIdB; btScalar m_friction; - btScalar m_restitution; btScalar m_jacDiagABInv; - btScalar m_penetration; - + union + { + int m_numConsecutiveRowsPerKernel; + btScalar m_unusedPadding0; + }; + union + { + int m_frictionIndex; + btScalar m_unusedPadding1; + }; + union + { + int m_solverBodyIdA; + btScalar m_unusedPadding2; + }; + union + { + int m_solverBodyIdB; + btScalar m_unusedPadding3; + }; - int m_constraintType; - int m_frictionIndex; - void* m_originalContactPoint; - int m_unusedPadding[1]; + union + { + void* m_originalContactPoint; + btScalar m_unusedPadding4; + }; + btScalar m_rhs; + btScalar m_cfm; + btScalar m_lowerLimit; + btScalar m_upperLimit; enum btSolverConstraintType { @@ -61,9 +85,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint }; }; - - - +typedef btAlignedObjectArray btConstraintArray; #endif //BT_SOLVER_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 6e8b552dbbc..3fa98ee4c88 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -19,13 +19,16 @@ subject to the following restrictions: static btRigidBody s_fixed(0, 0,0); +#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) + btTypedConstraint::btTypedConstraint(btTypedConstraintType type) :m_userConstraintType(-1), m_userConstraintId(-1), m_constraintType (type), m_rbA(s_fixed), m_rbB(s_fixed), -m_appliedImpulse(btScalar(0.)) +m_appliedImpulse(btScalar(0.)), +m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } @@ -35,7 +38,8 @@ m_userConstraintId(-1), m_constraintType (type), m_rbA(rbA), m_rbB(s_fixed), -m_appliedImpulse(btScalar(0.)) +m_appliedImpulse(btScalar(0.)), +m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); @@ -48,9 +52,63 @@ m_userConstraintId(-1), m_constraintType (type), m_rbA(rbA), m_rbB(rbB), -m_appliedImpulse(btScalar(0.)) +m_appliedImpulse(btScalar(0.)), +m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } + +//----------------------------------------------------------------------------- + +btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact) +{ + if(lowLim > uppLim) + { + return btScalar(1.0f); + } + else if(lowLim == uppLim) + { + return btScalar(0.0f); + } + btScalar lim_fact = btScalar(1.0f); + btScalar delta_max = vel / timeFact; + if(delta_max < btScalar(0.0f)) + { + if((pos >= lowLim) && (pos < (lowLim - delta_max))) + { + lim_fact = (lowLim - pos) / delta_max; + } + else if(pos < lowLim) + { + lim_fact = btScalar(0.0f); + } + else + { + lim_fact = btScalar(1.0f); + } + } + else if(delta_max > btScalar(0.0f)) + { + if((pos <= uppLim) && (pos > (uppLim - delta_max))) + { + lim_fact = (uppLim - pos) / delta_max; + } + else if(pos > uppLim) + { + lim_fact = btScalar(0.0f); + } + else + { + lim_fact = btScalar(1.0f); + } + } + else + { + lim_fact = btScalar(0.0f); + } + return lim_fact; +} // btTypedConstraint::getMotorFactor() + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index c50ec6ec579..78a770231b3 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -18,6 +18,11 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btScalar.h" +#include "btSolverConstraint.h" +struct btSolverBody; + + + enum btTypedConstraintType { @@ -48,6 +53,7 @@ protected: btRigidBody& m_rbA; btRigidBody& m_rbB; btScalar m_appliedImpulse; + btScalar m_dbgDrawSize; public: @@ -55,13 +61,55 @@ public: btTypedConstraint(btTypedConstraintType type); virtual ~btTypedConstraint() {}; btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); - btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); + struct btConstraintInfo1 { + int m_numConstraintRows,nub; + }; + + struct btConstraintInfo2 { + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + btScalar fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + btScalar *m_constraintError,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + btScalar *m_lowerLimit,*m_upperLimit; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + }; + + virtual void buildJacobian() = 0; - virtual void solveConstraint(btScalar timeStep) = 0; + virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) + { + } + virtual void getInfo1 (btConstraintInfo1* info)=0; + + virtual void getInfo2 (btConstraintInfo2* info)=0; + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0; + + btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); + const btRigidBody& getRigidBodyA() const { return m_rbA; @@ -94,7 +142,7 @@ public: { m_userConstraintId = uid; } - + int getUserConstraintId() const { return m_userConstraintId; @@ -114,7 +162,16 @@ public: { return m_constraintType; } - + + void setDbgDrawSize(btScalar dbgDrawSize) + { + m_dbgDrawSize = dbgDrawSize; + } + btScalar getDbgDrawSize() + { + return m_dbgDrawSize; + } + }; #endif //TYPED_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp index c2fd71d67fe..47addbac45b 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp @@ -38,7 +38,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" @@ -110,16 +110,16 @@ void plDeleteDynamicsWorld(plDynamicsWorldHandle world) void plStepSimulation(plDynamicsWorldHandle world, plReal timeStep) { btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - assert(dynamicsWorld); + btAssert(dynamicsWorld); dynamicsWorld->stepSimulation(timeStep); } void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) { btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - assert(dynamicsWorld); + btAssert(dynamicsWorld); btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - assert(body); + btAssert(body); dynamicsWorld->addRigidBody(body); } @@ -127,9 +127,9 @@ void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) { btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - assert(dynamicsWorld); + btAssert(dynamicsWorld); btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - assert(body); + btAssert(body); dynamicsWorld->removeRigidBody(body); } @@ -142,7 +142,7 @@ plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionSh trans.setIdentity(); btVector3 localInertia(0,0,0); btCollisionShape* shape = reinterpret_cast( cshape); - assert(shape); + btAssert(shape); if (mass) { shape->calculateLocalInertia(mass,localInertia); @@ -158,7 +158,7 @@ plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionSh void plDeleteRigidBody(plRigidBodyHandle cbody) { btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody); - assert(body); + btAssert(body); btAlignedFree( body); } @@ -255,20 +255,20 @@ void plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z) (void)colShape; btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE); btConvexHullShape* convexHullShape = reinterpret_cast( cshape); - convexHullShape->addPoint(btPoint3(x,y,z)); + convexHullShape->addPoint(btVector3(x,y,z)); } void plDeleteShape(plCollisionShapeHandle cshape) { btCollisionShape* shape = reinterpret_cast( cshape); - assert(shape); + btAssert(shape); btAlignedFree(shape); } void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling) { btCollisionShape* shape = reinterpret_cast( cshape); - assert(shape); + btAssert(shape); btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]); shape->setLocalScaling(scaling); } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 745694e2f70..ba48f404929 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -29,6 +29,11 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" +#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" +#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" +#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" //for debug rendering #include "BulletCollision/CollisionShapes/btBoxShape.h" @@ -51,6 +56,9 @@ subject to the following restrictions: #include "BulletDynamics/Vehicle/btRaycastVehicle.h" #include "BulletDynamics/Vehicle/btVehicleRaycaster.h" #include "BulletDynamics/Vehicle/btWheelInfo.h" +//character +#include "BulletDynamics/Character/btCharacterControllerInterface.h" + #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btMotionState.h" @@ -110,7 +118,6 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) btRigidBody* body = btRigidBody::upcast(colObj); if (body) { - btTransform predictedTrans; if (body->getActivationState() != ISLAND_SLEEPING) { if (body->isKinematicObject()) @@ -125,6 +132,7 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) void btDiscreteDynamicsWorld::debugDrawWorld() { + BT_PROFILE("debugDrawWorld"); if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) { @@ -144,13 +152,30 @@ void btDiscreteDynamicsWorld::debugDrawWorld() } } } + bool drawConstraints = false; + if (getDebugDrawer()) + { + int mode = getDebugDrawer()->getDebugMode(); + if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + { + drawConstraints = true; + } + } + if(drawConstraints) + { + for(int i = getNumConstraints()-1; i>=0 ;i--) + { + btTypedConstraint* constraint = getConstraint(i); + debugDrawConstraint(constraint); + } + } + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) { int i; - //todo: iterate over awake simulation islands! for ( i=0;igetDebugMode() & btIDebugDraw::DBG_DrawAabb)) { - btPoint3 minAabb,maxAabb; + btVector3 minAabb,maxAabb; btVector3 colorvec(1,0,0); colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); @@ -220,7 +245,7 @@ void btDiscreteDynamicsWorld::debugDrawWorld() void btDiscreteDynamicsWorld::clearForces() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;igetMotionState() && !body->isStaticOrKinematicObject()) + { + //we need to call the update at least once, even for sleeping objects + //otherwise the 'graphics' transform never updates properly + ///@todo: add 'dirty' flag + //if (body->getActivationState() != ISLAND_SLEEPING) + { + btTransform interpolatedTransform; + btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform); + body->getMotionState()->setWorldTransform(interpolatedTransform); + } + } +} + void btDiscreteDynamicsWorld::synchronizeMotionStates() { + BT_PROFILE("synchronizeMotionStates"); { //todo: iterate over awake simulation islands! for ( int i=0;igetMotionState() && !body->isStaticOrKinematicObject()) - { - //we need to call the update at least once, even for sleeping objects - //otherwise the 'graphics' transform never updates properly - //so todo: add 'dirty' flag - //if (body->getActivationState() != ISLAND_SLEEPING) - { - btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); - } - } + if (body) + synchronizeSingleMotionState(body); } } @@ -327,7 +361,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, //process some debugging flags if (getDebugDrawer()) { - gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + btIDebugDraw* debugDrawer = getDebugDrawer (); + gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; } if (numSimulationSubSteps) { @@ -392,7 +427,8 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) ///update vehicle simulation updateVehicles(timeStep); - + + updateCharacters(timeStep); updateActivationState( timeStep ); @@ -423,13 +459,6 @@ btVector3 btDiscreteDynamicsWorld::getGravity () const void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) { - //remove all constraints too - while (body->getNumConstraintRefs()) - { - btTypedConstraint* constraint = body->getConstraintRef(0); - removeConstraint(constraint); - } - removeCollisionObject(body); } @@ -475,6 +504,20 @@ void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) } } +void btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep) +{ + BT_PROFILE("updateCharacters"); + + for ( int i=0;ipreStep (this); + character->playerStep (this,timeStep); + } +} + + + void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { BT_PROFILE("updateActivationState"); @@ -539,6 +582,17 @@ void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) m_vehicles.remove(vehicle); } +void btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character) +{ + m_characters.push_back(character); +} + +void btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character) +{ + m_characters.remove(character); +} + + SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { int islandId; @@ -611,8 +665,11 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { if (islandId<0) { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + if (numManifolds + m_numConstraints) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } } else { //also add all non-contact constraints/joints for this island @@ -658,7 +715,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) sortedConstraints[i] = m_constraints[i]; } -// assert(0); +// btAssert(0); @@ -671,7 +728,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback); m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); } @@ -722,21 +779,27 @@ class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConve btCollisionObject* m_me; btScalar m_allowedPenetration; btOverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; public: - btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : + btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA,toA), m_allowedPenetration(0.0f), m_me(me), - m_pairCache(pairCache) + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) { if (convexResult.m_hitCollisionObject == m_me) - return 1.0; + return 1.0f; + + //ignore result if there is no contact response + if(!convexResult.m_hitCollisionObject->hasContactResponse()) + return 1.0f; btVector3 linVelA,linVelB; linVelA = m_convexToWorld-m_convexFromWorld; @@ -757,23 +820,29 @@ public: return false; ///don't do CCD when the collision filters are not matching - if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0)) + if (!ClosestConvexResultCallback::needsCollision(proxy0)) return false; - ///don't do CCD when there are already contact points (touching contact/penetration) - btAlignedObjectArray manifoldArray; - btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); - if (collisionPair) + btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + + //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 + if (m_dispatcher->needsResponse(m_me,otherObj)) { - if (collisionPair->m_algorithm) + ///don't do CCD when there are already contact points (touching contact/penetration) + btAlignedObjectArray manifoldArray; + btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); + if (collisionPair) { - manifoldArray.resize(0); - collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); - for (int j=0;jm_algorithm) { - btPersistentManifold* manifold = manifoldArray[j]; - if (manifold->getNumContacts()>0) - return false; + manifoldArray.resize(0); + collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); + for (int j=0;jgetNumContacts()>0) + return false; + } } } } @@ -811,9 +880,13 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { gNumClampedCcdMotions++; - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache()); + btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); btConvexShape* convexShape = static_cast(body->getCollisionShape()); btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast(body->getCollisionShape()); + + sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { @@ -833,6 +906,8 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) + + void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { BT_PROFILE("predictUnconstraintMotion"); @@ -844,14 +919,12 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { if (!body->isStaticOrKinematicObject()) { - if (body->isActive()) - { - body->integrateVelocities( timeStep); - //damping - body->applyDamping(timeStep); + + body->integrateVelocities( timeStep); + //damping + body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); } } } @@ -1103,7 +1176,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, { btConcaveShape* concaveMesh = (btConcaveShape*) shape; - //todo pass camera, for some culling + ///@todo pass camera, for some culling? no -> we are not a graphics lib btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); @@ -1132,7 +1205,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, int i; for (i=0;igetNumEdges();i++) { - btPoint3 a,b; + btVector3 a,b; polyshape->getEdge(i,a,b); btVector3 wa = worldTransform * a; btVector3 wb = worldTransform * b; @@ -1148,6 +1221,189 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, } +void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) +{ + bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; + bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; + btScalar dbgDrawSize = constraint->getDbgDrawSize(); + if(dbgDrawSize <= btScalar(0.f)) + { + return; + } + + switch(constraint->getConstraintType()) + { + case POINT2POINT_CONSTRAINT_TYPE: + { + btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; + btTransform tr; + tr.setIdentity(); + btVector3 pivot = p2pC->getPivotInA(); + pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + getDebugDrawer()->drawTransform(tr, dbgDrawSize); + // that ideally should draw the same frame + pivot = p2pC->getPivotInB(); + pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + } + break; + case HINGE_CONSTRAINT_TYPE: + { + btHingeConstraint* pHinge = (btHingeConstraint*)constraint; + btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + btScalar minAng = pHinge->getLowerLimit(); + btScalar maxAng = pHinge->getUpperLimit(); + if(minAng == maxAng) + { + break; + } + bool drawSect = true; + if(minAng > maxAng) + { + minAng = btScalar(0.f); + maxAng = SIMD_2_PI; + drawSect = false; + } + if(drawLimits) + { + btVector3& center = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); + } + } + break; + case CONETWIST_CONSTRAINT_TYPE: + { + btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; + btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + //const btScalar length = btScalar(5); + const btScalar length = dbgDrawSize; + static int nSegments = 8*4; + btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); + btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); + pPrev = tr * pPrev; + for (int i=0; iGetPointForAngle(fAngleInRadians, length); + pCur = tr * pCur; + getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); + + if (i%(nSegments/8) == 0) + getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); + + pPrev = pCur; + } + btScalar tws = pCT->getTwistSpan(); + btScalar twa = pCT->getTwistAngle(); + bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); + if(useFrameB) + { + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + } + else + { + tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + } + btVector3 pivot = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis1 = tr.getBasis().getColumn(1); + getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); + + } + } + break; + case D6_CONSTRAINT_TYPE: + { + btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; + btTransform tr = p6DOF->getCalculatedTransformA(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformB(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + tr = p6DOF->getCalculatedTransformA(); + const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); + btVector3 up = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; + btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; + btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; + btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); + axis = tr.getBasis().getColumn(1); + btScalar ay = p6DOF->getAngle(1); + btScalar az = p6DOF->getAngle(2); + btScalar cy = btCos(ay); + btScalar sy = btSin(ay); + btScalar cz = btCos(az); + btScalar sz = btSin(az); + btVector3 ref; + ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; + ref[1] = -sz*axis[0] + cz*axis[1]; + ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; + tr = p6DOF->getCalculatedTransformB(); + btVector3 normal = -tr.getBasis().getColumn(0); + btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; + btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; + if(minFi > maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); + } + else if(minFi < maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); + } + tr = p6DOF->getCalculatedTransformA(); + btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; + btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; + getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); + } + } + break; + case SLIDER_CONSTRAINT_TYPE: + { + btSliderConstraint* pSlider = (btSliderConstraint*)constraint; + btTransform tr = pSlider->getCalculatedTransformA(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pSlider->getCalculatedTransformB(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + btTransform tr = pSlider->getCalculatedTransformA(); + btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); + btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); + getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis = tr.getBasis().getColumn(1); + btScalar a_min = pSlider->getLowerAngLimit(); + btScalar a_max = pSlider->getUpperAngLimit(); + const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true); + } + } + break; + default : + break; + } + return; +} // btDiscreteDynamicsWorld::debugDrawConstraint() + + + + + void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) @@ -1176,3 +1432,5 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const { return m_constraints[index]; } + + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index d9e2652aaf6..34d4e6b353f 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -26,6 +26,7 @@ class btTypedConstraint; class btRaycastVehicle; +class btCharacterControllerInterface; class btIDebugDraw; #include "LinearMath/btAlignedObjectArray.h" @@ -42,7 +43,6 @@ protected: btAlignedObjectArray m_constraints; - btVector3 m_gravity; //for variable timesteps @@ -54,6 +54,9 @@ protected: btAlignedObjectArray m_vehicles; + + btAlignedObjectArray m_characters; + int m_profileTimings; @@ -61,24 +64,26 @@ protected: virtual void integrateTransforms(btScalar timeStep); - void calculateSimulationIslands(); + virtual void calculateSimulationIslands(); - void solveConstraints(btContactSolverInfo& solverInfo); + virtual void solveConstraints(btContactSolverInfo& solverInfo); void updateActivationState(btScalar timeStep); void updateVehicles(btScalar timeStep); + void updateCharacters(btScalar timeStep); + void startProfiling(btScalar timeStep); virtual void internalSingleStepSimulation( btScalar timeStep); - void synchronizeMotionStates(); - void saveKinematicState(btScalar timeStep); + virtual void saveKinematicState(btScalar timeStep); void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); + public: @@ -91,13 +96,23 @@ public: virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); - void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); + virtual void synchronizeMotionStates(); + + ///this can be useful to synchronize a single rigid body -> graphics object + void synchronizeSingleMotionState(btRigidBody* body); - void removeConstraint(btTypedConstraint* constraint); + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); - void addVehicle(btRaycastVehicle* vehicle); + virtual void removeConstraint(btTypedConstraint* constraint); - void removeVehicle(btRaycastVehicle* vehicle); + virtual void addVehicle(btRaycastVehicle* vehicle); + + virtual void removeVehicle(btRaycastVehicle* vehicle); + + virtual void addCharacter(btCharacterControllerInterface* character); + + virtual void removeCharacter(btCharacterControllerInterface* character); + btSimulationIslandManager* getSimulationIslandManager() { @@ -114,7 +129,6 @@ public: return this; } - virtual void setGravity(const btVector3& gravity); virtual btVector3 getGravity () const; @@ -126,6 +140,8 @@ public: void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + void debugDrawConstraint(btTypedConstraint* constraint); + virtual void debugDrawWorld(); virtual void setConstraintSolver(btConstraintSolver* solver); @@ -152,6 +168,7 @@ public: virtual void setNumTasks(int numTasks) { + (void) numTasks; } }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 929e24d337c..2d90e212f7a 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -23,6 +23,7 @@ class btTypedConstraint; class btRaycastVehicle; class btConstraintSolver; class btDynamicsWorld; +class btCharacterControllerInterface; /// Type for the callback for each tick typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); @@ -75,11 +76,18 @@ public: virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} + virtual void addCharacter(btCharacterControllerInterface* character) {(void)character;} + + virtual void removeCharacter(btCharacterControllerInterface* character) {(void)character;} + + //once a rigidbody is added to the dynamics world, it will get this gravity assigned //existing rigidbodies in the world get gravity assigned too, during this method virtual void setGravity(const btVector3& gravity) = 0; virtual btVector3 getGravity () const = 0; + virtual void synchronizeMotionStates() = 0; + virtual void addRigidBody(btRigidBody* body) = 0; virtual void removeRigidBody(btRigidBody* body) = 0; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index 93d70de39d8..a2b8a7dade4 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -45,8 +45,8 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); m_angularFactor = btScalar(1.); - m_anisotropicFriction.setValue(1.f,1.f,1.f); m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)), m_linearDamping = btScalar(0.); @@ -125,6 +125,7 @@ void btRigidBody::setGravity(const btVector3& acceleration) { m_gravity = acceleration * (btScalar(1.0) / m_inverseMass); } + m_gravity_acceleration = acceleration; } @@ -144,8 +145,17 @@ void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping void btRigidBody::applyDamping(btScalar timeStep) { + //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 + //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway + +//#define USE_OLD_DAMPING_METHOD 1 +#ifdef USE_OLD_DAMPING_METHOD m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); +#else + m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep); + m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep); +#endif if (m_additionalDamping) { diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index 4596f90a00f..ec570cab875 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -17,7 +17,6 @@ subject to the following restrictions: #define RIGIDBODY_H #include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btPoint3.h" #include "LinearMath/btTransform.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" @@ -31,7 +30,7 @@ extern btScalar gDeactivationTime; extern bool gDisableDeactivation; -///btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. +///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. ///It is recommended for performance and memory use to share btCollisionShape objects whenever possible. ///There are 3 types of rigid bodies: ///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics. @@ -49,6 +48,7 @@ class btRigidBody : public btCollisionObject btScalar m_angularFactor; btVector3 m_gravity; + btVector3 m_gravity_acceleration; btVector3 m_invInertiaLocal; btVector3 m_totalForce; btVector3 m_totalTorque; @@ -75,7 +75,7 @@ class btRigidBody : public btCollisionObject public: - ///btRigidBodyConstructionInfo provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. + ///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument) ///You can use the motion state to synchronize the world transform between physics and graphics objects. ///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state, @@ -182,7 +182,7 @@ public: const btVector3& getGravity() const { - return m_gravity; + return m_gravity_acceleration; } void setDamping(btScalar lin_damping, btScalar ang_damping); @@ -232,8 +232,18 @@ public: { m_totalForce += force; } + + const btVector3& getTotalForce() + { + return m_totalForce; + }; + + const btVector3& getTotalTorque() + { + return m_totalTorque; + }; - const btVector3& getInvInertiaDiagLocal() + const btVector3& getInvInertiaDiagLocal() const { return m_invInertiaLocal; }; @@ -303,7 +313,7 @@ public: void updateInertiaTensor(); - const btPoint3& getCenterOfMassPosition() const { + const btVector3& getCenterOfMassPosition() const { return m_worldTransform.getOrigin(); } btQuaternion getOrientation() const; @@ -321,15 +331,12 @@ public: inline void setLinearVelocity(const btVector3& lin_vel) { - assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT); m_linearVelocity = lin_vel; } - inline void setAngularVelocity(const btVector3& ang_vel) { - assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT); - { - m_angularVelocity = ang_vel; - } + inline void setAngularVelocity(const btVector3& ang_vel) + { + m_angularVelocity = ang_vel; } btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const @@ -353,7 +360,7 @@ public: - SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const + SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const { btVector3 r0 = pos - getCenterOfMassPosition(); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 3be04d1a4ad..3f6141463c3 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -97,7 +97,7 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b void btSimpleDynamicsWorld::clearForces() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;iisActive() && (!body->isStaticObject())) { - btPoint3 minAabb,maxAabb; + btVector3 minAabb,maxAabb; colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); btBroadphaseInterface* bp = getBroadphase(); bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); @@ -210,7 +210,7 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) void btSimpleDynamicsWorld::synchronizeMotionStates() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;icastRay(source,target,rayResults); @@ -188,7 +188,7 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; wheel.m_raycastInfo.m_isInContact = true; - wheel.m_raycastInfo.m_groundObject = &s_fixedObject;//todo for driving on dynamic/movable objects!; + wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!; //wheel.m_raycastInfo.m_groundObject = object; @@ -359,7 +359,7 @@ void btRaycastVehicle::updateVehicle( btScalar step ) void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel) { - assert(wheel>=0 && wheel < getNumWheels()); + btAssert(wheel>=0 && wheel < getNumWheels()); btWheelInfo& wheelInfo = getWheelInfo(wheel); wheelInfo.m_steering = steering; @@ -375,7 +375,7 @@ btScalar btRaycastVehicle::getSteeringValue(int wheel) const void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) { - assert(wheel>=0 && wheel < getNumWheels()); + btAssert(wheel>=0 && wheel < getNumWheels()); btWheelInfo& wheelInfo = getWheelInfo(wheel); wheelInfo.m_engineForce = force; } @@ -691,7 +691,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; - rel_pos[2] *= wheelInfo.m_rollInfluence; + rel_pos[m_indexForwardAxis] *= wheelInfo.m_rollInfluence; m_chassisBody->applyImpulse(sideImp,rel_pos); //apply friction impulse on the ground diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h index 8361dcabe4b..bfe0d7df2fb 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -180,7 +180,18 @@ public: //not yet } - virtual void solveConstraint(btScalar timeStep) + virtual void getInfo1 (btConstraintInfo1* info) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } + + virtual void getInfo2 (btConstraintInfo2* info) + { + btAssert(0); + } + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { (void)timeStep; //not yet diff --git a/extern/bullet2/src/BulletSoftBody/CMakeLists.txt b/extern/bullet2/src/BulletSoftBody/CMakeLists.txt index a725e10ff77..dbd87afea38 100644 --- a/extern/bullet2/src/BulletSoftBody/CMakeLists.txt +++ b/extern/bullet2/src/BulletSoftBody/CMakeLists.txt @@ -3,19 +3,41 @@ INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src } ) -ADD_LIBRARY(LibBulletSoftBody +SET(BulletSoftBody_SRCS btSoftBody.cpp - btSoftBody.h btSoftBodyHelpers.cpp - btSparseSDF.h - btSoftBodyHelpers.h btSoftBodyRigidBodyCollisionConfiguration.cpp btSoftRigidCollisionAlgorithm.cpp - btSoftRigidCollisionAlgorithm.h btSoftSoftCollisionAlgorithm.cpp - btSoftSoftCollisionAlgorithm.h btSoftBodyConcaveCollisionAlgorithm.cpp + btSoftRigidDynamicsWorld.cpp +) + +SET(BulletSoftBody_HDRS + btSoftBody.h + btSparseSDF.h + btSoftBodyHelpers.h + btSoftRigidCollisionAlgorithm.h + btSoftSoftCollisionAlgorithm.h btSoftBodyConcaveCollisionAlgorithm.h btSoftRigidDynamicsWorld.h - btSoftRigidDynamicsWorld.cpp ) + + + +ADD_LIBRARY(BulletSoftBody ${BulletSoftBody_SRCS} ${BulletSoftBody_HDRS}) +SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletSoftBody BulletDynamics) +ENDIF (BUILD_SHARED_LIBS) + +IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + INSTALL(TARGETS BulletSoftBody DESTINATION lib) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") +ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + +IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES PUBLIC_HEADER "${BulletSoftBody_HDRS}") +ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp index 1ec668c9c78..ee810c54082 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -150,98 +150,98 @@ bool btSoftBody::checkFace(int node0,int node1,int node2) const // btSoftBody::Material* btSoftBody::appendMaterial() { -Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material(); -if(m_materials.size()>0) - *pm=*m_materials[0]; + Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material(); + if(m_materials.size()>0) + *pm=*m_materials[0]; else - ZeroInitialize(*pm); -m_materials.push_back(pm); -return(pm); + ZeroInitialize(*pm); + m_materials.push_back(pm); + return(pm); } // void btSoftBody::appendNote( const char* text, - const btVector3& o, - const btVector4& c, - Node* n0, - Node* n1, - Node* n2, - Node* n3) -{ -Note n; -ZeroInitialize(n); -n.m_rank = 0; -n.m_text = text; -n.m_offset = o; -n.m_coords[0] = c.x(); -n.m_coords[1] = c.y(); -n.m_coords[2] = c.z(); -n.m_coords[3] = c.w(); -n.m_nodes[0] = n0;n.m_rank+=n0?1:0; -n.m_nodes[1] = n1;n.m_rank+=n1?1:0; -n.m_nodes[2] = n2;n.m_rank+=n2?1:0; -n.m_nodes[3] = n3;n.m_rank+=n3?1:0; -m_notes.push_back(n); + const btVector3& o, + const btVector4& c, + Node* n0, + Node* n1, + Node* n2, + Node* n3) +{ + Note n; + ZeroInitialize(n); + n.m_rank = 0; + n.m_text = text; + n.m_offset = o; + n.m_coords[0] = c.x(); + n.m_coords[1] = c.y(); + n.m_coords[2] = c.z(); + n.m_coords[3] = c.w(); + n.m_nodes[0] = n0;n.m_rank+=n0?1:0; + n.m_nodes[1] = n1;n.m_rank+=n1?1:0; + n.m_nodes[2] = n2;n.m_rank+=n2?1:0; + n.m_nodes[3] = n3;n.m_rank+=n3?1:0; + m_notes.push_back(n); } // void btSoftBody::appendNote( const char* text, - const btVector3& o, - Node* feature) + const btVector3& o, + Node* feature) { -appendNote(text,o,btVector4(1,0,0,0),feature); + appendNote(text,o,btVector4(1,0,0,0),feature); } // void btSoftBody::appendNote( const char* text, - const btVector3& o, - Link* feature) + const btVector3& o, + Link* feature) { -static const btScalar w=1/(btScalar)2; -appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0], - feature->m_n[1]); + static const btScalar w=1/(btScalar)2; + appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0], + feature->m_n[1]); } - + // void btSoftBody::appendNote( const char* text, - const btVector3& o, - Face* feature) + const btVector3& o, + Face* feature) { -static const btScalar w=1/(btScalar)3; -appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0], - feature->m_n[1], - feature->m_n[2]); + static const btScalar w=1/(btScalar)3; + appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0], + feature->m_n[1], + feature->m_n[2]); } // void btSoftBody::appendNode( const btVector3& x,btScalar m) { -if(m_nodes.capacity()==m_nodes.size()) + if(m_nodes.capacity()==m_nodes.size()) { - pointersToIndices(); - m_nodes.reserve(m_nodes.size()*2+1); - indicesToPointers(); + pointersToIndices(); + m_nodes.reserve(m_nodes.size()*2+1); + indicesToPointers(); } -const btScalar margin=getCollisionShape()->getMargin(); -m_nodes.push_back(Node()); -Node& n=m_nodes[m_nodes.size()-1]; -ZeroInitialize(n); -n.m_x = x; -n.m_q = n.m_x; -n.m_im = m>0?1/m:0; -n.m_material = m_materials[0]; -n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); + const btScalar margin=getCollisionShape()->getMargin(); + m_nodes.push_back(Node()); + Node& n=m_nodes[m_nodes.size()-1]; + ZeroInitialize(n); + n.m_x = x; + n.m_q = n.m_x; + n.m_im = m>0?1/m:0; + n.m_material = m_materials[0]; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); } // void btSoftBody::appendLink(int model,Material* mat) { -Link l; -if(model>=0) - l=m_links[model]; + Link l; + if(model>=0) + l=m_links[model]; else { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; } -m_links.push_back(l); + m_links.push_back(l); } // @@ -273,12 +273,12 @@ void btSoftBody::appendLink( Node* node0, // void btSoftBody::appendFace(int model,Material* mat) { -Face f; -if(model>=0) + Face f; + if(model>=0) { f=m_faces[model]; } else { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; } -m_faces.push_back(f); + m_faces.push_back(f); } // @@ -306,9 +306,9 @@ void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) } // -void btSoftBody::appendAnchor(int node,btRigidBody* body,bool disableCollisionWithBody=false) +void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies) { - if (disableCollisionWithBody) + if (disableCollisionBetweenLinkedBodies) { if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size()) { @@ -327,54 +327,54 @@ void btSoftBody::appendAnchor(int node,btRigidBody* body,bool disableCollision // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1) { -LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); -pj->m_bodies[0] = body0; -pj->m_bodies[1] = body1; -pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; -pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; -pj->m_cfm = specs.cfm; -pj->m_erp = specs.erp; -pj->m_split = specs.split; -m_joints.push_back(pj); + LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; + m_joints.push_back(pj); } // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body) { -appendLinearJoint(specs,m_clusters[0],body); + appendLinearJoint(specs,m_clusters[0],body); } // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) { -appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); + appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); } // void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1) { -AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); -pj->m_bodies[0] = body0; -pj->m_bodies[1] = body1; -pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; -pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; -pj->m_cfm = specs.cfm; -pj->m_erp = specs.erp; -pj->m_split = specs.split; -pj->m_icontrol = specs.icontrol; -m_joints.push_back(pj); + AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; + pj->m_icontrol = specs.icontrol; + m_joints.push_back(pj); } // void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body) { -appendAngularJoint(specs,m_clusters[0],body); + appendAngularJoint(specs,m_clusters[0],body); } // void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) { -appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); + appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); } // @@ -463,8 +463,8 @@ void btSoftBody::setTotalMass(btScalar mass,bool fromfaces) { const Face& f=m_faces[i]; const btScalar twicearea=AreaOf( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); + f.m_n[1]->m_x, + f.m_n[2]->m_x); for(int j=0;j<3;++j) { f.m_n[j]->m_im+=twicearea; @@ -494,13 +494,17 @@ void btSoftBody::setTotalDensity(btScalar density) void btSoftBody::transform(const btTransform& trs) { const btScalar margin=getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + for(int i=0,ni=m_nodes.size();igetMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + for(int i=0,ni=m_nodes.size();i0 ? - 1/(m_nodes[i].m_im*tmass) : - kmass/tmass; + 1/(m_nodes[i].m_im*tmass) : + kmass/tmass; } /* Pos */ const btVector3 com=evaluateCom(); @@ -578,16 +585,16 @@ void btSoftBody::setPose(bool bvolume,bool bframe) m_pose.m_scl.setIdentity(); /* Aqq */ m_pose.m_aqq[0] = - m_pose.m_aqq[1] = - m_pose.m_aqq[2] = btVector3(0,0,0); + m_pose.m_aqq[1] = + m_pose.m_aqq[2] = btVector3(0,0,0); for( i=0,ni=m_nodes.size();im_nodes.size();im_nodes.size();im_nodes[i]->m_x*cluster->m_masses[i]; + com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i]; } -return(com*cluster->m_imass); + return(com*cluster->m_imass); } // btVector3 btSoftBody::clusterCom(int cluster) const { -return(clusterCom(m_clusters[cluster])); + return(clusterCom(m_clusters[cluster])); } // btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) { -return(cluster->m_lv+cross(cluster->m_av,rpos)); + return(cluster->m_lv+cross(cluster->m_av,rpos)); } // void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) { -const btVector3 li=cluster->m_imass*impulse; -const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); -cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; -cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; -cluster->m_nvimpulses++; + const btVector3 li=cluster->m_imass*impulse; + const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); + cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; + cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + cluster->m_nvimpulses++; } // void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) { -const btVector3 li=cluster->m_imass*impulse; -const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); -cluster->m_dimpulses[0]+=li; -cluster->m_dimpulses[1]+=ai; -cluster->m_ndimpulses++; + const btVector3 li=cluster->m_imass*impulse; + const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); + cluster->m_dimpulses[0]+=li; + cluster->m_dimpulses[1]+=ai; + cluster->m_ndimpulses++; } // void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse) { -if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); -if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); + if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); + if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); } // void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse) { -const btVector3 ai=cluster->m_invwi*impulse; -cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; -cluster->m_nvimpulses++; + const btVector3 ai=cluster->m_invwi*impulse; + cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + cluster->m_nvimpulses++; } // void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse) { -const btVector3 ai=cluster->m_invwi*impulse; -cluster->m_dimpulses[1]+=ai; -cluster->m_ndimpulses++; + const btVector3 ai=cluster->m_invwi*impulse; + cluster->m_dimpulses[1]+=ai; + cluster->m_ndimpulses++; } // void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse) { -if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); -if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); + if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); + if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); } // void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) { -cluster->m_dimpulses[0]+=impulse*cluster->m_imass; -cluster->m_ndimpulses++; + cluster->m_dimpulses[0]+=impulse*cluster->m_imass; + cluster->m_ndimpulses++; } // @@ -762,9 +769,9 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat) // void btSoftBody::randomizeConstraints() { -unsigned long seed=243703; + unsigned long seed=243703; #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff) -int i,ni; + int i,ni; for(i=0,ni=m_links.size();im_leaf) m_cdbvt.remove(c->m_leaf); -c->~Cluster(); -btAlignedFree(c); -m_clusters.remove(c); + Cluster* c=m_clusters[index]; + if(c->m_leaf) m_cdbvt.remove(c->m_leaf); + c->~Cluster(); + btAlignedFree(c); + m_clusters.remove(c); } // void btSoftBody::releaseClusters() { -while(m_clusters.size()>0) releaseCluster(0); + while(m_clusters.size()>0) releaseCluster(0); } // int btSoftBody::generateClusters(int k,int maxiterations) { -int i; -releaseClusters(); -m_clusters.resize(btMin(k,m_nodes.size())); -for(i=0;im_collide= true; - } -k=m_clusters.size(); -if(k>0) - { - /* Initialize */ - btAlignedObjectArray centers; - btVector3 cog(0,0,0); - int i; - for(i=0;im_nodes.push_back(&m_nodes[i]); - } - cog/=(btScalar)m_nodes.size(); - centers.resize(k,cog); - /* Iterate */ - const btScalar slope=16; - bool changed; - int iterations=0; - do { - const btScalar w=2-btMin(1,iterations/slope); - changed=false; - iterations++; - int i; - - for(i=0;im_collide= true; + } + k=m_clusters.size(); + if(k>0) + { + /* Initialize */ + btAlignedObjectArray centers; + btVector3 cog(0,0,0); + int i; + for(i=0;im_nodes.push_back(&m_nodes[i]); + } + cog/=(btScalar)m_nodes.size(); + centers.resize(k,cog); + /* Iterate */ + const btScalar slope=16; + bool changed; + int iterations=0; + do { + const btScalar w=2-btMin(1,iterations/slope); + changed=false; + iterations++; + int i; + + for(i=0;im_nodes.size();++j) + btVector3 c(0,0,0); + for(int j=0;jm_nodes.size();++j) { - c+=m_clusters[i]->m_nodes[j]->m_x; + c+=m_clusters[i]->m_nodes[j]->m_x; } - if(m_clusters[i]->m_nodes.size()) + if(m_clusters[i]->m_nodes.size()) { - c /= (btScalar)m_clusters[i]->m_nodes.size(); - c = centers[i]+(c-centers[i])*w; - changed |= ((c-centers[i]).length2()>SIMD_EPSILON); - centers[i] = c; - m_clusters[i]->m_nodes.resize(0); + c /= (btScalar)m_clusters[i]->m_nodes.size(); + c = centers[i]+(c-centers[i])*w; + changed |= ((c-centers[i]).length2()>SIMD_EPSILON); + centers[i] = c; + m_clusters[i]->m_nodes.resize(0); } } - for(i=0;im_nodes.push_back(&m_nodes[i]); + m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]); } } while(changed&&(iterations cids; - cids.resize(m_nodes.size(),-1); - for(i=0;i cids; + cids.resize(m_nodes.size(),-1); + for(i=0;im_nodes.size();++j) + for(int j=0;jm_nodes.size();++j) { - cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; + cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; } } - for(i=0;im_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size()) + if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size()) { - m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); + m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); } } } } } - /* Master */ - if(m_clusters.size()>1) + /* Master */ + if(m_clusters.size()>1) { - Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - pmaster->m_collide = false; - pmaster->m_nodes.reserve(m_nodes.size()); - for(int i=0;im_nodes.push_back(&m_nodes[i]); - m_clusters.push_back(pmaster); - btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); + Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); + pmaster->m_collide = false; + pmaster->m_nodes.reserve(m_nodes.size()); + for(int i=0;im_nodes.push_back(&m_nodes[i]); + m_clusters.push_back(pmaster); + btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); } - /* Terminate */ - for(i=0;im_nodes.size()==0) + if(m_clusters[i]->m_nodes.size()==0) { - releaseCluster(i--); + releaseCluster(i--); } } - - initializeClusters(); - updateClusters(); - return(m_clusters.size()); + + initializeClusters(); + updateClusters(); + + //for self-collision + m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size()); + { + for (int c0=0;c0m_clusterIndex=c0; + for (int c1=0;c1m_nodes.size();i++) + { + for (int j=0;jm_nodes.size();j++) + { + if (cla->m_nodes[i] == clb->m_nodes[j]) + { + connected=true; + break; + } + } + } + m_clusterConnectivity[c0+c1*m_clusters.size()]=connected; + } + } + } + + return(m_clusters.size()); } -return(0); + return(0); } // void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) { -const Node* nbase = &m_nodes[0]; -int ncount = m_nodes.size(); -btSymMatrix edges(ncount,-2); -int newnodes=0; -int i,j,k,ni; + const Node* nbase = &m_nodes[0]; + int ncount = m_nodes.size(); + btSymMatrix edges(ncount,-2); + int newnodes=0; + int i,j,k,ni; -/* Filter out */ -for(i=0;iEval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x))) + if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x))) { - btSwap(m_links[i],m_links[m_links.size()-1]); - m_links.pop_back();--i; + btSwap(m_links[i],m_links[m_links.size()-1]); + m_links.pop_back();--i; } } } -/* Fill edges */ -for(i=0;i0) + Node& a=m_nodes[i]; + Node& b=m_nodes[j]; + const btScalar t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary); + if(t>0) { - const btVector3 x=Lerp(a.m_x,b.m_x,t); - const btVector3 v=Lerp(a.m_v,b.m_v,t); - btScalar m=0; - if(a.m_im>0) + const btVector3 x=Lerp(a.m_x,b.m_x,t); + const btVector3 v=Lerp(a.m_v,b.m_v,t); + btScalar m=0; + if(a.m_im>0) { - if(b.m_im>0) + if(b.m_im>0) { - const btScalar ma=1/a.m_im; - const btScalar mb=1/b.m_im; - const btScalar mc=Lerp(ma,mb,t); - const btScalar f=(ma+mb)/(ma+mb+mc); - a.m_im=1/(ma*f); - b.m_im=1/(mb*f); - m=mc*f; + const btScalar ma=1/a.m_im; + const btScalar mb=1/b.m_im; + const btScalar mc=Lerp(ma,mb,t); + const btScalar f=(ma+mb)/(ma+mb+mc); + a.m_im=1/(ma*f); + b.m_im=1/(mb*f); + m=mc*f; } else { a.m_im/=0.5;m=1/a.m_im; } } else { - if(b.m_im>0) + if(b.m_im>0) { b.m_im/=0.5;m=1/b.m_im; } else - m=0; + m=0; } - appendNode(x,m); - edges(i,j)=m_nodes.size()-1; - m_nodes[edges(i,j)].m_v=v; - ++newnodes; + appendNode(x,m); + edges(i,j)=m_nodes.size()-1; + m_nodes[edges(i,j)].m_v=v; + ++newnodes; } } } } -nbase=&m_nodes[0]; -/* Refine links */ -for(i=0,ni=m_links.size();i0) + const int ni=edges(idx[0],idx[1]); + if(ni>0) { - appendLink(i); - Link* pft[]={ &m_links[i], - &m_links[m_links.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[0]]; - pft[0]->m_n[1]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[1]]; + appendLink(i); + Link* pft[]={ &m_links[i], + &m_links[m_links.size()-1]}; + pft[0]->m_n[0]=&m_nodes[idx[0]]; + pft[0]->m_n[1]=&m_nodes[ni]; + pft[1]->m_n[0]=&m_nodes[ni]; + pft[1]->m_n[1]=&m_nodes[idx[1]]; } } } -/* Refine faces */ -for(i=0;i0) + const int ni=edges(idx[j],idx[k]); + if(ni>0) { - appendFace(i); - const int l=(k+1)%3; - Face* pft[]={ &m_faces[i], - &m_faces[m_faces.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[l]]; - pft[0]->m_n[1]=&m_nodes[idx[j]]; - pft[0]->m_n[2]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[k]]; - pft[1]->m_n[2]=&m_nodes[idx[l]]; - appendLink(ni,idx[l],pft[0]->m_material); - --i;break; + appendFace(i); + const int l=(k+1)%3; + Face* pft[]={ &m_faces[i], + &m_faces[m_faces.size()-1]}; + pft[0]->m_n[0]=&m_nodes[idx[l]]; + pft[0]->m_n[1]=&m_nodes[idx[j]]; + pft[0]->m_n[2]=&m_nodes[ni]; + pft[1]->m_n[0]=&m_nodes[ni]; + pft[1]->m_n[1]=&m_nodes[idx[k]]; + pft[1]->m_n[2]=&m_nodes[idx[l]]; + appendLink(ni,idx[l],pft[0]->m_material); + --i;break; } } } } -/* Cut */ -if(cut) + /* Cut */ + if(cut) { - btAlignedObjectArray cnodes; - const int pcount=ncount; - int i; - ncount=m_nodes.size(); - cnodes.resize(ncount,0); - /* Nodes */ - for(i=0;i=pcount)||(btFabs(ifn->Eval(x)) cnodes; + const int pcount=ncount; + int i; + ncount=m_nodes.size(); + cnodes.resize(ncount,0); + /* Nodes */ + for(i=0;i=pcount)||(btFabs(ifn->Eval(x))0) { m*=0.5;m_nodes[i].m_im/=0.5; } - appendNode(x,m); - cnodes[i]=m_nodes.size()-1; - m_nodes[cnodes[i]].m_v=v; + const btVector3 v=m_nodes[i].m_v; + btScalar m=getMass(i); + if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; } + appendNode(x,m); + cnodes[i]=m_nodes.size()-1; + m_nodes[cnodes[i]].m_v=v; } } - nbase=&m_nodes[0]; - /* Links */ - for(i=0,ni=m_links.size();iEval(m_nodes[id[0]].m_x)Eval(m_nodes[id[0]].m_x)Eval(m_nodes[id[1]].m_x)Eval(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x)Eval(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x) ranks; - btAlignedObjectArray todelete; - ranks.resize(nnodes,0); - for(i=0,ni=m_links.size();i ranks; + btAlignedObjectArray todelete; + ranks.resize(nnodes,0); + for(i=0,ni=m_links.size();i=0;--i) +#if 0 + for(i=nnodes-1;i>=0;--i) { - if(!ranks[i]) todelete.push_back(i); + if(!ranks[i]) todelete.push_back(i); } - if(todelete.size()) + if(todelete.size()) { - btAlignedObjectArray& map=ranks; - for(int i=0;i& map=ranks; + for(int i=0;im_v=v; -pn[1]->m_v=v; -for(i=0,ni=m_links.size();im_n[1]=pn[mtch]; - pft[1]->m_n[0]=pn[1-mtch]; - done=true; - } - } -for(i=0,ni=m_faces.size();im_v=v; + pn[1]->m_v=v; + for(i=0,ni=m_links.size();im_n[1]=pn[mtch]; + pft[1]->m_n[0]=pn[1-mtch]; + done=true; + } + } + for(i=0,ni=m_faces.size();im_n[l]=pn[mtch]; - pft[1]->m_n[k]=pn[1-mtch]; - appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); - appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + appendFace(i); + Face* pft[]={&m_faces[i],&m_faces[m_faces.size()-1]}; + pft[0]->m_n[l]=pn[mtch]; + pft[1]->m_n[k]=pn[1-mtch]; + appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); } } } -if(!done) + if(!done) { - m_ndbvt.remove(pn[0]->m_leaf); - m_ndbvt.remove(pn[1]->m_leaf); - m_nodes.pop_back(); - m_nodes.pop_back(); + m_ndbvt.remove(pn[0]->m_leaf); + m_ndbvt.remove(pn[1]->m_leaf); + m_nodes.pop_back(); + m_nodes.pop_back(); } -return(done); + return(done); } // -bool btSoftBody::rayCast(const btVector3& org, - const btVector3& dir, - sRayCast& results, - btScalar maxtime) +bool btSoftBody::rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) { -if(m_faces.size()&&m_fdbvt.empty()) initializeFaceTree(); -results.body = this; -results.time = maxtime; -results.feature = eFeature::None; -results.index = -1; -return(rayCast(org,dir,results.time,results.feature,results.index,false)!=0); + if(m_faces.size()&&m_fdbvt.empty()) + initializeFaceTree(); + + results.body = this; + results.fraction = 1.f; + results.feature = eFeature::None; + results.index = -1; + + return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0); } // void btSoftBody::setSolver(eSolverPresets::_ preset) { -m_cfg.m_vsequence.clear(); -m_cfg.m_psequence.clear(); -m_cfg.m_dsequence.clear(); -switch(preset) + m_cfg.m_vsequence.clear(); + m_cfg.m_psequence.clear(); + m_cfg.m_dsequence.clear(); + switch(preset) { case eSolverPresets::Positions: - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - m_cfg.m_psequence.push_back(ePSolver::Linear); - break; + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + m_cfg.m_psequence.push_back(ePSolver::Linear); + break; case eSolverPresets::Velocities: - m_cfg.m_vsequence.push_back(eVSolver::Linear); - - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - - m_cfg.m_dsequence.push_back(ePSolver::Linear); - break; + m_cfg.m_vsequence.push_back(eVSolver::Linear); + + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + + m_cfg.m_dsequence.push_back(ePSolver::Linear); + break; } } @@ -1282,11 +1320,11 @@ void btSoftBody::predictMotion(btScalar dt) updateConstants(); m_fdbvt.clear(); if(m_cfg.collisions&fCollision::VF_SS) - { + { initializeFaceTree(); - } + } } - + /* Prepare */ m_sst.sdt = dt*m_cfg.timescale; m_sst.isdt = 1/m_sst.sdt; @@ -1310,45 +1348,48 @@ void btSoftBody::predictMotion(btScalar dt) /* Bounds */ updateBounds(); /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; for(i=0,ni=m_nodes.size();im_v+ - f.m_n[1]->m_v+ - f.m_n[2]->m_v)/3; + f.m_n[1]->m_v+ + f.m_n[2]->m_v)/3; + vol = VolumeOf(f,m_sst.radmrg); m_fdbvt.update( f.m_leaf, - VolumeOf(f,m_sst.radmrg), - v*m_sst.velmrg, - m_sst.updmrg); - } + vol, + v*m_sst.velmrg, + m_sst.updmrg); } + } /* Pose */ updatePose(); /* Match */ if(m_pose.m_bframe&&(m_cfg.kMT>0)) - { + { const btMatrix3x3 posetrs=m_pose.m_rot; for(int i=0,ni=m_nodes.size();i0) - { + { const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com; n.m_x=Lerp(n.m_x,x,m_cfg.kMT); - } } } + } /* Clear contacts */ m_rcontacts.resize(0); m_scontacts.resize(0); @@ -1361,104 +1402,104 @@ void btSoftBody::predictMotion(btScalar dt) // void btSoftBody::solveConstraints() { -/* Apply clusters */ -applyClusters(false); -/* Prepare links */ + /* Apply clusters */ + applyClusters(false); + /* Prepare links */ -int i,ni; + int i,ni; -for(i=0,ni=m_links.size();im_q-l.m_n[0]->m_q; - l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); + Link& l=m_links[i]; + l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q; + l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); } -/* Prepare anchors */ -for(i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; - a.m_c0 = ImpulseMatrix( m_sst.sdt, - a.m_node->m_im, - a.m_body->getInvMass(), - a.m_body->getInvInertiaTensorWorld(), - ra); - a.m_c1 = ra; - a.m_c2 = m_sst.sdt*a.m_node->m_im; - a.m_body->activate(); + Anchor& a=m_anchors[i]; + const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local; + a.m_c0 = ImpulseMatrix( m_sst.sdt, + a.m_node->m_im, + a.m_body->getInvMass(), + a.m_body->getInvInertiaTensorWorld(), + ra); + a.m_c1 = ra; + a.m_c2 = m_sst.sdt*a.m_node->m_im; + a.m_body->activate(); } -/* Solve velocities */ -if(m_cfg.viterations>0) + /* Solve velocities */ + if(m_cfg.viterations>0) { - /* Solve */ - for(int isolve=0;isolve0) + /* Solve positions */ + if(m_cfg.piterations>0) { - for(int isolve=0;isolve0) + /* Solve drift */ + if(m_cfg.diterations>0) { - const btScalar vcf=m_cfg.kVCF*m_sst.isdt; - for(i=0,ni=m_nodes.size();i& bodies) { -const int nb=bodies.size(); -int iterations=0; -int i; + const int nb=bodies.size(); + int iterations=0; + int i; -for(i=0;im_cfg.citerations); + iterations=btMax(iterations,bodies[i]->m_cfg.citerations); } -for(i=0;iprepareClusters(iterations); + bodies[i]->prepareClusters(iterations); } -for(i=0;isolveClusters(sor); + bodies[j]->solveClusters(sor); } } -for(i=0;icleanupClusters(); + bodies[i]->cleanupClusters(); } } @@ -1506,48 +1547,54 @@ void btSoftBody::integrateMotion() } // - btSoftBody::RayCaster::RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt) +btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) { -o = org; -d = dir; -mint = mxt; -face = 0; -tests = 0; + m_rayFrom = rayFrom; + m_rayNormalizedDirection = (rayTo-rayFrom); + m_rayTo = rayTo; + m_mint = mxt; + m_face = 0; + m_tests = 0; } // -void btSoftBody::RayCaster::Process(const btDbvtNode* leaf) +void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) { -btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; -const btScalar t=rayTriangle( o,d, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); -if((t>0)&&(tdata; + const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + m_mint); + if((t>0)&&(tteps)&&(tceps) && (dot(n,cross(b-hit,c-hit))>ceps) && (dot(n,cross(c-hit,a-hit))>ceps)) @@ -1569,9 +1616,9 @@ void btSoftBody::pointersToIndices() for(i=0,ni=m_nodes.size();idata=*(void**)&i; - } + } } for(i=0,ni=m_links.size();idata=*(void**)&i; - } + } } for(i=0,ni=m_anchors.size();idata=&m_nodes[i]; - } + } } for(i=0,ni=m_links.size();idata=&m_faces[i]; - } + } } for(i=0,ni=m_anchors.size();im_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); + + const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + mint); if(t>0) - { + { ++cnt; if(!bcountonly) - { + { feature=btSoftBody::eFeature::Face; index=i; mint=t; - } } } } - else - {/* Use dbvt */ - RayCaster collider(org,dir,mint); - btDbvt::collideRAY(m_fdbvt.m_root,org,dir,collider); - if(collider.face) - { - mint=collider.mint; + } + else + {/* Use dbvt */ + RayFromToCaster collider(rayFrom,rayTo,mint); + + btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider); + if(collider.m_face) + { + mint=collider.m_mint; feature=btSoftBody::eFeature::Face; - index=(int)(collider.face-&m_faces[0]); + index=(int)(collider.m_face-&m_faces[0]); cnt=1; - } } + } return(cnt); } // void btSoftBody::initializeFaceTree() { -m_fdbvt.clear(); -for(int i=0;igetCollisionShape(); - const btTransform& wtr=prb->getInterpolationWorldTransform(); + btCollisionShape* shp=colObj->getCollisionShape(); + btRigidBody* tmpRigid = btRigidBody::upcast(colObj); + const btTransform& wtr=tmpRigid? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform(); btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), - shp, - nrm, - margin); + shp, + nrm, + margin); if(dst<0) { - cti.m_body = prb; + cti.m_colObj = colObj; cti.m_normal = wtr.getBasis()*nrm; cti.m_offset = -dot( cti.m_normal, - x-cti.m_normal*dst); + x-cti.m_normal*dst); return(true); } return(false); @@ -1751,7 +1804,7 @@ void btSoftBody::updateNormals() { btSoftBody::Face& f=m_faces[i]; const btVector3 n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x, - f.m_n[2]->m_x-f.m_n[0]->m_x); + f.m_n[2]->m_x-f.m_n[0]->m_x); f.m_normal=n.normalized(); f.m_n[0]->m_n+=n; f.m_n[1]->m_n+=n; @@ -1769,28 +1822,28 @@ void btSoftBody::updateNormals() void btSoftBody::updateBounds() { if(m_ndbvt.m_root) - { + { const btVector3& mins=m_ndbvt.m_root->volume.Mins(); const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); const btScalar csm=getCollisionShape()->getMargin(); const btVector3 mrg=btVector3( csm, - csm, - csm)*1; // ??? to investigate... + csm, + csm)*1; // ??? to investigate... m_bounds[0]=mins-mrg; m_bounds[1]=maxs+mrg; - if(0!=getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); - } + if(0!=getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); } - else - { + } + else + { m_bounds[0]= - m_bounds[1]=btVector3(0,0,0); - } + m_bounds[1]=btVector3(0,0,0); + } } @@ -1821,12 +1874,12 @@ void btSoftBody::updatePose() pose.m_rot=r; pose.m_scl=pose.m_aqq*r.transpose()*Apq; if(m_cfg.maxvolume>1) - { + { const btScalar idet=Clamp( 1/pose.m_scl.determinant(), - 1,m_cfg.maxvolume); + 1,m_cfg.maxvolume); pose.m_scl=Mul(pose.m_scl,idet); - } - + } + } } @@ -1881,53 +1934,53 @@ void btSoftBody::initializeClusters() { int i; -for( i=0;im_im>0?1/c.m_nodes[j]->m_im:0; - c.m_imass += c.m_masses[j]; + c.m_masses[j] = c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0; + c.m_imass += c.m_masses[j]; } - c.m_imass = 1/c.m_imass; - c.m_com = btSoftBody::clusterCom(&c); - c.m_lv = btVector3(0,0,0); - c.m_av = btVector3(0,0,0); - c.m_leaf = 0; - /* Inertia */ - btMatrix3x3& ii=c.m_locii; - ii[0]=ii[1]=ii[2]=btVector3(0,0,0); - { - int i,ni; + c.m_imass = 1/c.m_imass; + c.m_com = btSoftBody::clusterCom(&c); + c.m_lv = btVector3(0,0,0); + c.m_av = btVector3(0,0,0); + c.m_leaf = 0; + /* Inertia */ + btMatrix3x3& ii=c.m_locii; + ii[0]=ii[1]=ii[2]=btVector3(0,0,0); + { + int i,ni; - for(i=0,ni=c.m_nodes.size();im_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=c.m_masses[i]; - ii[0][0] += m*(q[1]+q[2]); - ii[1][1] += m*(q[0]+q[2]); - ii[2][2] += m*(q[0]+q[1]); - ii[0][1] -= m*k[0]*k[1]; - ii[0][2] -= m*k[0]*k[2]; - ii[1][2] -= m*k[1]*k[2]; - } - } - ii[1][0]=ii[0][1]; - ii[2][0]=ii[0][2]; - ii[2][1]=ii[1][2]; - ii=ii.inverse(); - /* Frame */ - c.m_framexform.setIdentity(); - c.m_framexform.setOrigin(c.m_com); - c.m_framerefs.resize(c.m_nodes.size()); - { - int i; - for(i=0;im_x-c.m_com; + const btVector3 q=k*k; + const btScalar m=c.m_masses[i]; + ii[0][0] += m*(q[1]+q[2]); + ii[1][1] += m*(q[0]+q[2]); + ii[2][2] += m*(q[0]+q[1]); + ii[0][1] -= m*k[0]*k[1]; + ii[0][2] -= m*k[0]*k[2]; + ii[1][2] -= m*k[1]*k[2]; + } + } + ii[1][0]=ii[0][1]; + ii[2][0]=ii[0][2]; + ii[2][1]=ii[1][2]; + ii=ii.inverse(); + /* Frame */ + c.m_framexform.setIdentity(); + c.m_framexform.setOrigin(c.m_com); + c.m_framerefs.resize(c.m_nodes.size()); + { + int i; + for(i=0;im_x-c.m_com; + c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com; } } } @@ -1936,49 +1989,49 @@ for( i=0;im_x-c.m_com; - const btVector3& b=c.m_framerefs[i]; - m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b; + const btVector3 a=c.m_nodes[i]->m_x-c.m_com; + const btVector3& b=c.m_framerefs[i]; + m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b; } - PolarDecompose(m,r,s); - c.m_framexform.setOrigin(c.m_com); - c.m_framexform.setBasis(r); - /* Inertia */ - #if 1/* Constant */ - c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); - #else - #if 0/* Sphere */ + PolarDecompose(m,r,s); + c.m_framexform.setOrigin(c.m_com); + c.m_framexform.setBasis(r); + /* Inertia */ +#if 1/* Constant */ + c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); +#else +#if 0/* Sphere */ const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass); const btVector3 inertia(rk,rk,rk); const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0, - btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0, - btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); - + btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0, + btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); + c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose(); - #else/* Actual */ +#else/* Actual */ c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0); for(int i=0;im_x-c.m_com; const btVector3 q=k*k; const btScalar m=1/c.m_nodes[i]->m_im; @@ -1988,74 +2041,79 @@ for(i=0;im_v*c.m_masses[i]; - c.m_lv += v; - c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v); + int i; + + for(i=0;im_v*c.m_masses[i]; + c.m_lv += v; + c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v); + } } - } - c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); - c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping); - c.m_vimpulses[0] = - c.m_vimpulses[1] = btVector3(0,0,0); - c.m_dimpulses[0] = - c.m_dimpulses[1] = btVector3(0,0,0); - c.m_nvimpulses = 0; - c.m_ndimpulses = 0; - /* Matching */ - if(c.m_matching>0) + c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); + c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping); + c.m_vimpulses[0] = + c.m_vimpulses[1] = btVector3(0,0,0); + c.m_dimpulses[0] = + c.m_dimpulses[1] = btVector3(0,0,0); + c.m_nvimpulses = 0; + c.m_ndimpulses = 0; + /* Matching */ + if(c.m_matching>0) { - for(int j=0;jm_x; - btVector3 mx=mi; - for(int j=1;jm_x; + btVector3 mx=mi; + for(int j=1;jm_x); - mx.setMax(c.m_nodes[j]->m_x); + mi.setMin(c.m_nodes[j]->m_x); + mx.setMax(c.m_nodes[j]->m_x); } - const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx); - if(c.m_leaf) - m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); + ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx); + if(c.m_leaf) + m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); else - c.m_leaf=m_cdbvt.insert(bounds,&c); + c.m_leaf=m_cdbvt.insert(bounds,&c); } } } + + } + + + // void btSoftBody::cleanupClusters() { -for(int i=0;iTerminate(m_sst.sdt); - if(m_joints[i]->m_delete) + m_joints[i]->Terminate(m_sst.sdt); + if(m_joints[i]->m_delete) { - btAlignedFree(m_joints[i]); - m_joints.remove(m_joints[i--]); + btAlignedFree(m_joints[i]); + m_joints.remove(m_joints[i--]); } } } @@ -2063,9 +2121,9 @@ for(int i=0;iPrepare(m_sst.sdt,iterations); + m_joints[i]->Prepare(m_sst.sdt,iterations); } } @@ -2073,50 +2131,51 @@ for(int i=0;iSolve(m_sst.sdt,sor); + m_joints[i]->Solve(m_sst.sdt,sor); } } // void btSoftBody::applyClusters(bool drift) { -BT_PROFILE("ApplyClusters"); -const btScalar f0=m_sst.sdt; -const btScalar f1=f0/2; -btAlignedObjectArray deltas; -btAlignedObjectArray weights; -deltas.resize(m_nodes.size(),btVector3(0,0,0)); -weights.resize(m_nodes.size(),0); -int i; + BT_PROFILE("ApplyClusters"); + const btScalar f0=m_sst.sdt; + const btScalar f1=f0/2; + btAlignedObjectArray deltas; + btAlignedObjectArray weights; + deltas.resize(m_nodes.size(),btVector3(0,0,0)); + weights.resize(m_nodes.size(),0); + int i; -if(drift) + if(drift) { - for(i=0;im_x; - const btScalar q=c.m_masses[j]; - deltas[idx] += (v+cross(w,x-c.m_com))*q; - weights[idx] += q; + const int idx=int(c.m_nodes[j]-&m_nodes[0]); + const btVector3& x=c.m_nodes[j]->m_x; + const btScalar q=c.m_masses[j]; + deltas[idx] += (v+cross(w,x-c.m_com))*q; + weights[idx] += q; } } } @@ -2131,18 +2190,21 @@ void btSoftBody::dampClusters() { int i; -for(i=0;i0) + Cluster& c=*m_clusters[i]; + if(c.m_ndamping>0) { - for(int j=0;j0) + Node& n=*c.m_nodes[j]; + if(n.m_im>0) { - const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com); - n.m_v += c.m_ndamping*(vx-n.m_v); + const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com); + if(vx.length2()<=n.m_v.length2()) + { + n.m_v += c.m_ndamping*(vx-n.m_v); + } } } } @@ -2152,154 +2214,155 @@ for(i=0;i0) + static const btScalar maxdrift=4; + Joint::Prepare(dt,iterations); + m_rpos[0] = m_bodies[0].xform()*m_refs[0]; + m_rpos[1] = m_bodies[1].xform()*m_refs[1]; + m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt; + m_rpos[0] -= m_bodies[0].xform().getOrigin(); + m_rpos[1] -= m_bodies[1].xform().getOrigin(); + m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0], + m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]); + if(m_split>0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; } -m_drift /=(btScalar)iterations; + m_drift /=(btScalar)iterations; } // void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor) { -const btVector3 va=m_bodies[0].velocity(m_rpos[0]); -const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); -const btVector3 vr=va-vb; -btSoftBody::Impulse impulse; -impulse.m_asVelocity = 1; -impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; -m_bodies[0].applyImpulse(-impulse,m_rpos[0]); -m_bodies[1].applyImpulse( impulse,m_rpos[1]); + const btVector3 va=m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); + const btVector3 vr=va-vb; + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; + m_bodies[0].applyImpulse(-impulse,m_rpos[0]); + m_bodies[1].applyImpulse( impulse,m_rpos[1]); } // void btSoftBody::LJoint::Terminate(btScalar dt) { -if(m_split>0) + if(m_split>0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); + m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); } } // void btSoftBody::AJoint::Prepare(btScalar dt,int iterations) { -static const btScalar maxdrift=SIMD_PI/16; -m_icontrol->Prepare(this); -Joint::Prepare(dt,iterations); -m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; -m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; -m_drift = NormalizeAny(cross(m_axis[1],m_axis[0])); -m_drift *= btMin(maxdrift,btAcos(Clamp(dot(m_axis[0],m_axis[1]),-1,+1))); -m_drift *= m_erp/dt; -m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); -if(m_split>0) + static const btScalar maxdrift=SIMD_PI/16; + m_icontrol->Prepare(this); + Joint::Prepare(dt,iterations); + m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; + m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; + m_drift = NormalizeAny(cross(m_axis[1],m_axis[0])); + m_drift *= btMin(maxdrift,btAcos(Clamp(dot(m_axis[0],m_axis[1]),-1,+1))); + m_drift *= m_erp/dt; + m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); + if(m_split>0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; } -m_drift /=(btScalar)iterations; + m_drift /=(btScalar)iterations; } // void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor) { -const btVector3 va=m_bodies[0].angularVelocity(); -const btVector3 vb=m_bodies[1].angularVelocity(); -const btVector3 vr=va-vb; -const btScalar sp=dot(vr,m_axis[0]); -const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); -btSoftBody::Impulse impulse; -impulse.m_asVelocity = 1; -impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor; -m_bodies[0].applyAImpulse(-impulse); -m_bodies[1].applyAImpulse( impulse); + const btVector3 va=m_bodies[0].angularVelocity(); + const btVector3 vb=m_bodies[1].angularVelocity(); + const btVector3 vr=va-vb; + const btScalar sp=dot(vr,m_axis[0]); + const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor; + m_bodies[0].applyAImpulse(-impulse); + m_bodies[1].applyAImpulse( impulse); } // void btSoftBody::AJoint::Terminate(btScalar dt) { -if(m_split>0) + if(m_split>0) { - m_bodies[0].applyDAImpulse(-m_sdrift); - m_bodies[1].applyDAImpulse( m_sdrift); + m_bodies[0].applyDAImpulse(-m_sdrift); + m_bodies[1].applyDAImpulse( m_sdrift); } } // void btSoftBody::CJoint::Prepare(btScalar dt,int iterations) { -Joint::Prepare(dt,iterations); -const bool dodrift=(m_life==0); -m_delete=(++m_life)>m_maxlife; -if(dodrift) + Joint::Prepare(dt,iterations); + const bool dodrift=(m_life==0); + m_delete=(++m_life)>m_maxlife; + if(dodrift) { - m_drift=m_drift*m_erp/dt; - if(m_split>0) + m_drift=m_drift*m_erp/dt; + if(m_split>0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; } - m_drift/=(btScalar)iterations; + m_drift/=(btScalar)iterations; } else { - m_drift=m_sdrift=btVector3(0,0,0); + m_drift=m_sdrift=btVector3(0,0,0); } } // void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) { -const btVector3 va=m_bodies[0].velocity(m_rpos[0]); -const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); -const btVector3 vrel=va-vb; -const btScalar rvac=dot(vrel,m_normal); -btSoftBody::Impulse impulse; -impulse.m_asVelocity = 1; -impulse.m_velocity = m_drift; -if(rvac<0) + const btVector3 va=m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); + const btVector3 vrel=va-vb; + const btScalar rvac=dot(vrel,m_normal); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_drift; + if(rvac<0) { - const btVector3 iv=m_normal*rvac; - const btVector3 fv=vrel-iv; - impulse.m_velocity += iv+fv*m_friction; + const btVector3 iv=m_normal*rvac; + const btVector3 fv=vrel-iv; + impulse.m_velocity += iv+fv*m_friction; } -impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; -m_bodies[0].applyImpulse(-impulse,m_rpos[0]); -m_bodies[1].applyImpulse( impulse,m_rpos[1]); + impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; + m_bodies[0].applyImpulse(-impulse,m_rpos[0]); + m_bodies[1].applyImpulse( impulse,m_rpos[1]); } // void btSoftBody::CJoint::Terminate(btScalar dt) { -if(m_split>0) + if(m_split>0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); + m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); } } // void btSoftBody::applyForces() { + BT_PROFILE("SoftBody applyForces"); const btScalar dt=m_sst.sdt; const btScalar kLF=m_cfg.kLF; @@ -2311,14 +2374,14 @@ void btSoftBody::applyForces() const bool as_pressure=kPR!=0; const bool as_volume=kVC>0; const bool as_aero= as_lift || - as_drag ; + as_drag ; const bool as_vaero= as_aero && - (m_cfg.aeromodel=btSoftBody::eAeroModel::F_TwoSided); + (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided); const bool use_medium= as_aero; const bool use_volume= as_pressure || - as_volume ; + as_volume ; btScalar volume=0; btScalar ivolumetp=0; btScalar dvolumetv=0; @@ -2447,12 +2510,10 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; - ///skip object that don't have collision response - if (!psb->getWorldInfo()->m_dispatcher->needsResponse(psb,c.m_cti.m_body)) - continue; - const sCti& cti=c.m_cti; - const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*dt; + btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj); + + const btVector3 va=tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); const btVector3 vb=c.m_node->m_x-c.m_node->m_q; const btVector3 vr=vb-va; const btScalar dn=dot(vr,cti.m_normal); @@ -2462,7 +2523,8 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) const btVector3 fv=vr-cti.m_normal*dn; const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst); c.m_node->m_x-=impulse*c.m_c2; - c.m_cti.m_body->applyImpulse(impulse,c.m_c1); + if (tmpRigid) + tmpRigid->applyImpulse(impulse,c.m_c1); } } } @@ -2470,32 +2532,32 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) // void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) { -for(int i=0,ni=psb->m_scontacts.size();im_scontacts.size();im_scontacts[i]; - const btVector3& nr=c.m_normal; - Node& n=*c.m_node; - Face& f=*c.m_face; - const btVector3 p=BaryEval( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - c.m_weights); - const btVector3 q=BaryEval( f.m_n[0]->m_q, - f.m_n[1]->m_q, - f.m_n[2]->m_q, - c.m_weights); - const btVector3 vr=(n.m_x-n.m_q)-(p-q); - btVector3 corr(0,0,0); - if(dot(vr,nr)<0) + const SContact& c=psb->m_scontacts[i]; + const btVector3& nr=c.m_normal; + Node& n=*c.m_node; + Face& f=*c.m_face; + const btVector3 p=BaryEval( f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + c.m_weights); + const btVector3 q=BaryEval( f.m_n[0]->m_q, + f.m_n[1]->m_q, + f.m_n[2]->m_q, + c.m_weights); + const btVector3 vr=(n.m_x-n.m_q)-(p-q); + btVector3 corr(0,0,0); + if(dot(vr,nr)<0) { - const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p)); - corr+=c.m_normal*j; + const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p)); + corr+=c.m_normal*j; } - corr -= ProjectOnPlane(vr,nr)*c.m_friction; - n.m_x += corr*c.m_cfm[0]; - f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); - f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); - f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); + corr -= ProjectOnPlane(vr,nr)*c.m_friction; + n.m_x += corr*c.m_cfm[0]; + f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); + f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); + f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); } } @@ -2522,107 +2584,114 @@ void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) // void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) { -for(int i=0,ni=psb->m_links.size();im_links.size();im_links[i]; - Node** n=l.m_n; - const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; - n[0]->m_v+= l.m_c3*(j*n[0]->m_im); - n[1]->m_v-= l.m_c3*(j*n[1]->m_im); + Link& l=psb->m_links[i]; + Node** n=l.m_n; + const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; + n[0]->m_v+= l.m_c3*(j*n[0]->m_im); + n[1]->m_v-= l.m_c3*(j*n[1]->m_im); } } // btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) { -switch(solver) + switch(solver) { case ePSolver::Anchors: return(&btSoftBody::PSolve_Anchors); case ePSolver::Linear: return(&btSoftBody::PSolve_Links); case ePSolver::RContacts: return(&btSoftBody::PSolve_RContacts); case ePSolver::SContacts: return(&btSoftBody::PSolve_SContacts); } -return(0); + return(0); } // btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) { -switch(solver) + switch(solver) { case eVSolver::Linear: return(&btSoftBody::VSolve_Links); } -return(0); + return(0); } // void btSoftBody::defaultCollisionHandler(btCollisionObject* pco) { -switch(m_cfg.collisions&fCollision::RVSmask) + switch(m_cfg.collisions&fCollision::RVSmask) { case fCollision::SDF_RS: { - btSoftColliders::CollideSDF_RS docollide; - btRigidBody* prb=btRigidBody::upcast(pco); - const btTransform wtr=prb->getInterpolationWorldTransform(); - const btTransform ctr=prb->getWorldTransform(); - const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); - const btScalar basemargin=getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), - mins, - maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(basemargin,basemargin,basemargin)); - docollide.psb = this; - docollide.prb = prb; - docollide.dynmargin = basemargin+timemargin; - docollide.stamargin = basemargin; - btDbvt::collideTV(m_ndbvt.m_root,volume,docollide); - } - break; + btSoftColliders::CollideSDF_RS docollide; + btRigidBody* prb1=btRigidBody::upcast(pco); + btTransform wtr=prb1 ? prb1->getInterpolationWorldTransform() : pco->getWorldTransform(); + + const btTransform ctr=pco->getWorldTransform(); + const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); + const btScalar basemargin=getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; + pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), + mins, + maxs); + volume=btDbvtVolume::FromMM(mins,maxs); + volume.Expand(btVector3(basemargin,basemargin,basemargin)); + docollide.psb = this; + docollide.m_colObj1 = pco; + docollide.m_rigidBody = prb1; + + docollide.dynmargin = basemargin+timemargin; + docollide.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); + } + break; case fCollision::CL_RS: { - btSoftColliders::CollideCL_RS collider; - collider.Process(this,btRigidBody::upcast(pco)); + btSoftColliders::CollideCL_RS collider; + collider.Process(this,pco); } - break; + break; } } // void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { -const int cf=m_cfg.collisions&psb->m_cfg.collisions; -switch(cf&fCollision::SVSmask) + const int cf=m_cfg.collisions&psb->m_cfg.collisions; + switch(cf&fCollision::SVSmask) { case fCollision::CL_SS: { - btSoftColliders::CollideCL_SS docollide; - docollide.Process(this,psb); + btSoftColliders::CollideCL_SS docollide; + docollide.Process(this,psb); } - break; + break; case fCollision::VF_SS: { - btSoftColliders::CollideVF_SS docollide; - /* common */ - docollide.mrg= getCollisionShape()->getMargin()+ - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - docollide.psb[0]=this; - docollide.psb[1]=psb; - btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - /* psb1 nodes vs psb0 faces */ - docollide.psb[0]=psb; - docollide.psb[1]=this; - btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - } - break; + //only self-collision for Cluster, not Vertex-Face yet + if (this!=psb) + { + btSoftColliders::CollideVF_SS docollide; + /* common */ + docollide.mrg= getCollisionShape()->getMargin()+ + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + docollide.psb[0]=this; + docollide.psb[1]=psb; + docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + docollide.psb[0]=psb; + docollide.psb[1]=this; + docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + } + break; } } diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.h b/extern/bullet2/src/BulletSoftBody/btSoftBody.h index 743462d259a..a62c21883c8 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.h @@ -18,7 +18,6 @@ subject to the following restrictions: #define _BT_SOFT_BODY_H #include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btPoint3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btIDebugDraw.h" #include "BulletDynamics/Dynamics/btRigidBody.h" @@ -45,7 +44,8 @@ struct btSoftBodyWorldInfo }; -/// btSoftBody is work-in-progress +///The btSoftBody is an class to simulate cloth and volumetric soft bodies. +///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject. class btSoftBody : public btCollisionObject { public: @@ -64,13 +64,13 @@ public: F_OneSided, ///Face normals are taken as it is END };}; - + ///eVSolver : velocities solvers struct eVSolver { enum _ { Linear, ///Linear solver END };}; - + ///ePSolver : positions solvers struct ePSolver { enum _ { Linear, ///Linear solver @@ -79,7 +79,7 @@ public: SContacts, ///Soft contacts solver END };}; - + ///eSolverPresets struct eSolverPresets { enum _ { Positions, @@ -87,7 +87,7 @@ public: Default = Positions, END };}; - + ///eFeature struct eFeature { enum _ { None, @@ -96,20 +96,20 @@ public: Face, END };}; - + typedef btAlignedObjectArray tVSolverArray; typedef btAlignedObjectArray tPSolverArray; - + // // Flags // - + ///fCollision struct fCollision { enum _ { RVSmask = 0x000f, ///Rigid versus soft mask SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - + SVSmask = 0x00f0, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling @@ -117,7 +117,7 @@ public: Default = SDF_RS, END };}; - + ///fMaterial struct fMaterial { enum _ { DebugDraw = 0x0001, /// Enable debug draw @@ -125,26 +125,26 @@ public: Default = DebugDraw, END };}; - + // // API Types // - + /* sRayCast */ struct sRayCast { btSoftBody* body; /// soft body eFeature::_ feature; /// feature type int index; /// feature index - btScalar time; /// time of impact (rayorg+raydir*time) + btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction) }; - + /* ImplicitFn */ struct ImplicitFn { virtual btScalar Eval(const btVector3& x)=0; }; - + // // Internal types // @@ -155,7 +155,7 @@ public: /* sCti is Softbody contact info */ struct sCti { - btRigidBody* m_body; /* Rigid body */ + btCollisionObject* m_colObj; /* Rigid body */ btVector3 m_normal; /* Outward normal */ btScalar m_offset; /* Offset from origin */ }; @@ -182,7 +182,7 @@ public: btScalar m_kVST; // Volume stiffness coefficient [0,1] int m_flags; // Flags }; - + /* Feature */ struct Feature : Element { @@ -293,12 +293,13 @@ public: btVector3 m_lv; btVector3 m_av; btDbvtNode* m_leaf; - btScalar m_ndamping; - btScalar m_ldamping; - btScalar m_adamping; + btScalar m_ndamping; /* Node damping */ + btScalar m_ldamping; /* Linear damping */ + btScalar m_adamping; /* Angular damping */ btScalar m_matching; bool m_collide; - Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} + int m_clusterIndex; + Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} }; /* Impulse */ struct Impulse @@ -309,109 +310,115 @@ public: int m_asDrift:1; Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {} Impulse operator -() const - { + { Impulse i=*this; i.m_velocity=-i.m_velocity; i.m_drift=-i.m_drift; return(i); - } + } Impulse operator*(btScalar x) const - { + { Impulse i=*this; i.m_velocity*=x; i.m_drift*=x; return(i); - } + } }; /* Body */ struct Body { - Cluster* m_soft; - btRigidBody* m_rigid; - Body() : m_soft(0),m_rigid(0) {} - Body(Cluster* p) : m_soft(p),m_rigid(0) {} - Body(btRigidBody* p) : m_soft(0),m_rigid(p) {} + Cluster* m_soft; + btRigidBody* m_rigid; + btCollisionObject* m_collisionObject; + + Body() : m_soft(0),m_rigid(0),m_collisionObject(0) {} + Body(Cluster* p) : m_soft(p),m_rigid(0),m_collisionObject(0) {} + Body(btCollisionObject* colObj) : m_soft(0),m_collisionObject(colObj) + { + m_rigid = btRigidBody::upcast(m_collisionObject); + } + void activate() const - { + { if(m_rigid) m_rigid->activate(); - } + } const btMatrix3x3& invWorldInertia() const - { + { static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0); if(m_rigid) return(m_rigid->getInvInertiaTensorWorld()); if(m_soft) return(m_soft->m_invwi); return(iwi); - } + } btScalar invMass() const - { + { if(m_rigid) return(m_rigid->getInvMass()); if(m_soft) return(m_soft->m_imass); return(0); - } + } const btTransform& xform() const - { + { static const btTransform identity=btTransform::getIdentity(); - if(m_rigid) return(m_rigid->getInterpolationWorldTransform()); + if(m_collisionObject) return(m_collisionObject->getInterpolationWorldTransform()); if(m_soft) return(m_soft->m_framexform); return(identity); - } + } btVector3 linearVelocity() const - { + { if(m_rigid) return(m_rigid->getLinearVelocity()); if(m_soft) return(m_soft->m_lv); return(btVector3(0,0,0)); - } + } btVector3 angularVelocity(const btVector3& rpos) const - { + { if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos)); if(m_soft) return(cross(m_soft->m_av,rpos)); return(btVector3(0,0,0)); - } + } btVector3 angularVelocity() const - { + { if(m_rigid) return(m_rigid->getAngularVelocity()); if(m_soft) return(m_soft->m_av); return(btVector3(0,0,0)); - } + } btVector3 velocity(const btVector3& rpos) const - { + { return(linearVelocity()+angularVelocity(rpos)); - } + } void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const - { + { if(m_rigid) m_rigid->applyImpulse(impulse,rpos); if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse); - } + } void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const - { + { if(m_rigid) m_rigid->applyImpulse(impulse,rpos); if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse); - } + } void applyImpulse(const Impulse& impulse,const btVector3& rpos) const - { + { if(impulse.m_asVelocity) applyVImpulse(impulse.m_velocity,rpos); if(impulse.m_asDrift) applyDImpulse(impulse.m_drift,rpos); - } + } void applyVAImpulse(const btVector3& impulse) const - { + { if(m_rigid) m_rigid->applyTorqueImpulse(impulse); if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); - } + } void applyDAImpulse(const btVector3& impulse) const - { + { if(m_rigid) m_rigid->applyTorqueImpulse(impulse); if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); - } + } void applyAImpulse(const Impulse& impulse) const - { + { if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); - } + } void applyDCImpulse(const btVector3& impulse) const - { + { if(m_rigid) m_rigid->applyCentralImpulse(impulse); if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); - } + } }; /* Joint */ struct Joint @@ -419,15 +426,15 @@ public: struct eType { enum _ { Linear, Angular, - Contact, + Contact };}; struct Specs - { - Specs() : erp(1),cfm(1),split(1) {} + { + Specs() : erp(1),cfm(1),split(1) {} btScalar erp; btScalar cfm; btScalar split; - }; + }; Body m_bodies[2]; btVector3 m_refs[2]; btScalar m_cfm; @@ -438,7 +445,7 @@ public: btMatrix3x3 m_massmatrix; bool m_delete; virtual ~Joint() {} - Joint() : m_delete(false) {} + Joint() : m_delete(false) {} virtual void Prepare(btScalar dt,int iterations); virtual void Solve(btScalar dt,btScalar sor)=0; virtual void Terminate(btScalar dt)=0; @@ -448,9 +455,9 @@ public: struct LJoint : Joint { struct Specs : Joint::Specs - { + { btVector3 position; - }; + }; btVector3 m_rpos[2]; void Prepare(btScalar dt,int iterations); void Solve(btScalar dt,btScalar sor); @@ -461,17 +468,17 @@ public: struct AJoint : Joint { struct IControl - { + { virtual void Prepare(AJoint*) {} virtual btScalar Speed(AJoint*,btScalar current) { return(current); } static IControl* Default() { static IControl def;return(&def); } - }; + }; struct Specs : Joint::Specs - { - Specs() : icontrol(IControl::Default()) {} + { + Specs() : icontrol(IControl::Default()) {} btVector3 axis; IControl* icontrol; - }; + }; btVector3 m_axis[2]; IControl* m_icontrol; void Prepare(btScalar dt,int iterations); @@ -534,23 +541,26 @@ public: btScalar radmrg; // radial margin btScalar updmrg; // Update margin }; - /* RayCaster */ - struct RayCaster : btDbvt::ICollide - { - btVector3 o; - btVector3 d; - btScalar mint; - Face* face; - int tests; - RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt); + /// RayFromToCaster takes a ray from, ray to (instead of direction!) + struct RayFromToCaster : btDbvt::ICollide + { + btVector3 m_rayFrom; + btVector3 m_rayTo; + btVector3 m_rayNormalizedDirection; + btScalar m_mint; + Face* m_face; + int m_tests; + RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); void Process(const btDbvtNode* leaf); - static inline btScalar rayTriangle(const btVector3& org, - const btVector3& dir, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt=SIMD_INFINITY); - }; + + static inline btScalar rayFromToTriangle(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt=SIMD_INFINITY); + }; // // Typedef's @@ -596,16 +606,19 @@ public: btDbvt m_fdbvt; // Faces tree btDbvt m_cdbvt; // Clusters tree tClusterArray m_clusters; // Clusters - - btTransform m_initialWorldTransform; //used to attach constraints etc. + + btAlignedObjectArraym_clusterConnectivity;//cluster connectivity, for self-collision + + btTransform m_initialWorldTransform; + // // Api // - + /* ctor */ btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, - const btVector3* x, - const btScalar* m); + const btVector3* x, + const btScalar* m); /* dtor */ virtual ~btSoftBody(); /* Check for existing link */ @@ -617,59 +630,60 @@ public: return m_worldInfo; } + ///@todo: avoid internal softbody shape hack and move collision code to collision library virtual void setCollisionShape(btCollisionShape* collisionShape) { - //don't do anything, due to the internal shape hack: todo: fix this + } bool checkLink( int node0, int node1) const; bool checkLink( const Node* node0, - const Node* node1) const; + const Node* node1) const; /* Check for existring face */ bool checkFace( int node0, - int node1, - int node2) const; + int node1, + int node2) const; /* Append material */ Material* appendMaterial(); /* Append note */ void appendNote( const char* text, - const btVector3& o, - const btVector4& c=btVector4(1,0,0,0), - Node* n0=0, - Node* n1=0, - Node* n2=0, - Node* n3=0); + const btVector3& o, + const btVector4& c=btVector4(1,0,0,0), + Node* n0=0, + Node* n1=0, + Node* n2=0, + Node* n3=0); void appendNote( const char* text, - const btVector3& o, - Node* feature); + const btVector3& o, + Node* feature); void appendNote( const char* text, - const btVector3& o, - Link* feature); + const btVector3& o, + Link* feature); void appendNote( const char* text, - const btVector3& o, - Face* feature); + const btVector3& o, + Face* feature); /* Append node */ void appendNode( const btVector3& x,btScalar m); /* Append link */ void appendLink(int model=-1,Material* mat=0); void appendLink( int node0, - int node1, - Material* mat=0, - bool bcheckexist=false); + int node1, + Material* mat=0, + bool bcheckexist=false); void appendLink( Node* node0, - Node* node1, - Material* mat=0, - bool bcheckexist=false); + Node* node1, + Material* mat=0, + bool bcheckexist=false); /* Append face */ void appendFace(int model=-1,Material* mat=0); void appendFace( int node0, - int node1, - int node2, - Material* mat=0); + int node1, + int node2, + Material* mat=0); /* Append anchor */ void appendAnchor( int node, - btRigidBody* body,bool disableCollision); + btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false); /* Append linear joint */ void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); @@ -682,7 +696,7 @@ public: void addForce( const btVector3& force); /* Add force (or gravity) to a node of the body */ void addForce( const btVector3& force, - int node); + int node); /* Add velocity to the entire body */ void addVelocity( const btVector3& velocity); @@ -691,17 +705,17 @@ public: /* Add velocity to a node of the body */ void addVelocity( const btVector3& velocity, - int node); + int node); /* Set mass */ void setMass( int node, - btScalar mass); + btScalar mass); /* Get mass */ btScalar getMass( int node) const; /* Get total mass */ btScalar getTotalMass() const; /* Set total mass (weighted by previous masses) */ void setTotalMass( btScalar mass, - bool fromfaces=false); + bool fromfaces=false); /* Set total density */ void setTotalDensity(btScalar density); /* Transform */ @@ -714,7 +728,7 @@ public: void scale( const btVector3& scl); /* Set current state as pose */ void setPose( bool bvolume, - bool bframe); + bool bframe); /* Return the volume */ btScalar getVolume() const; /* Cluster count */ @@ -734,7 +748,7 @@ public: static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse); /* Generate bending constraints based on distance in the adjency graph */ int generateBendingConstraints( int distance, - Material* mat=0); + Material* mat=0); /* Randomize constraints to reduce solver bias */ void randomizeConstraints(); /* Release clusters */ @@ -747,11 +761,11 @@ public: /* CutLink */ bool cutLink(int node0,int node1,btScalar position); bool cutLink(const Node* node0,const Node* node1,btScalar position); - /* Ray casting */ - bool rayCast(const btVector3& org, - const btVector3& dir, - sRayCast& results, - btScalar maxtime=SIMD_INFINITY); + + ///Ray casting using rayFrom and rayTo in worldspace, (not direction!) + bool rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results); /* Solver presets */ void setSolver(eSolverPresets::_ preset); /* predictMotion */ @@ -769,11 +783,11 @@ public: /* defaultCollisionHandlers */ void defaultCollisionHandler(btCollisionObject* pco); void defaultCollisionHandler(btSoftBody* psb); - + // // Cast // - + static const btSoftBody* upcast(const btCollisionObject* colObj) { if (colObj->getInternalType()==CO_SOFT_BODY) @@ -801,11 +815,12 @@ public: // void pointersToIndices(); void indicesToPointers(const int* map=0); - int rayCast(const btVector3& org,const btVector3& dir, - btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; + + int rayTest(const btVector3& rayFrom,const btVector3& rayTo, + btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; void initializeFaceTree(); btVector3 evaluateCom() const; - bool checkContact(btRigidBody* prb,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; + bool checkContact(btCollisionObject* colObj,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; void updateNormals(); void updateBounds(); void updatePose(); @@ -825,7 +840,7 @@ public: static void VSolve_Links(btSoftBody* psb,btScalar kst); static psolver_t getSolver(ePSolver::_ solver); static vsolver_t getSolver(eVSolver::_ solver); - + }; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index 61a57ea5da9..f334e15e0d3 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -32,7 +32,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletSoftBody/btSoftBody.h" -#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.3)//make this configurable +#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) : btCollisionAlgorithm(ci), @@ -50,27 +50,27 @@ btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm() btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): - m_dispatcher(dispatcher), - m_dispatchInfoPtr(0) +m_dispatcher(dispatcher), +m_dispatchInfoPtr(0) { m_softBody = (btSoftBody*) (isSwapped? body1:body0); m_triBody = isSwapped? body0:body1; - - // - // create the manifold from the dispatcher 'manifold pool' - // -// m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); - clearCache(); + // + // create the manifold from the dispatcher 'manifold pool' + // + // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); + + clearCache(); } btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() { clearCache(); -// m_dispatcher->releaseManifold( m_manifoldPtr ); - + // m_dispatcher->releaseManifold( m_manifoldPtr ); + } - + void btSoftBodyTriangleCallback::clearCache() { @@ -83,18 +83,18 @@ void btSoftBodyTriangleCallback::clearCache() delete tmp->m_childShape; } m_shapeCache.clear(); -}; +} void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) { - //just for debugging purposes + //just for debugging purposes //printf("triangle %d",m_triangleCount++); btCollisionObject* ob = static_cast(m_triBody); btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; - ///debug drawing of the overlapping triangles + ///debug drawing of the overlapping triangles if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) { btVector3 color(255,255,0); @@ -107,7 +107,7 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, btTriIndex triIndex(partId,triangleIndex,0); btHashKey triKey(triIndex.getUid()); - + btTriIndex* shapeIndex = m_shapeCache[triKey]; if (shapeIndex) { @@ -116,13 +116,13 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, //copy over user pointers to temporary shape tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); - + btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( tm ); - + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); - + colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); @@ -133,56 +133,56 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, //aabb filter is already applied! //btCollisionObject* colObj = static_cast(m_convexProxy->m_clientObject); - -// if (m_softBody->getCollisionShape()->getShapeType()== + + // if (m_softBody->getCollisionShape()->getShapeType()== { -// btVector3 other; + // btVector3 other; btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); normal.normalize(); normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; -// other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; -// other+=normal*22.f; - btVector3 pts[6] = {triangle[0], - triangle[1], - triangle[2], - triangle[0]-normal, - triangle[1]-normal, - triangle[2]-normal}; + // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; + // other+=normal*22.f; + btVector3 pts[6] = {triangle[0]+normal, + triangle[1]+normal, + triangle[2]+normal, + triangle[0]-normal, + triangle[1]-normal, + triangle[2]-normal}; btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); -// btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); - + // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); + //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); - // tm.setMargin(m_collisionMarginTriangle); - + // tm.setMargin(m_collisionMarginTriangle); + //copy over user pointers to temporary shape tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); - + btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( tm ); - + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); ///this should use the btDispatcher, so the actual registered algorithm is used // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); - // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); -// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); + // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - - + + ob->internalSetTemporaryCollisionShape( tmpShape ); triIndex.m_childShape = tm; m_shapeCache.insert(triKey,triIndex); } - + } @@ -194,7 +194,7 @@ void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMargin m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); m_resultOut = resultOut; - + btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax; m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax); btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5); @@ -217,8 +217,8 @@ void btSoftBodyConcaveCollisionAlgorithm::clearCache() void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { - - + + btCollisionObject* convexBody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; @@ -228,26 +228,26 @@ void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* b btCollisionObject* triOb = triBody; btConcaveShape* concaveShape = static_cast( triOb->getCollisionShape()); - - // if (convexBody->getCollisionShape()->isConvex()) + + // if (convexBody->getCollisionShape()->isConvex()) { btScalar collisionMarginTriangle = concaveShape->getMargin(); - -// resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); + + // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut); //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); -// m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); + // m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); - - // resultOut->refreshContactPoints(); - + + // resultOut->refreshContactPoints(); + } - + } } @@ -287,7 +287,7 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO btScalar m_ccdSphereRadius; btScalar m_hitFraction; - + LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) :m_ccdSphereFromTrans(from), @@ -296,8 +296,8 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO m_hitFraction(hitFraction) { } - - + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { (void)partId; @@ -327,9 +327,9 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO }; - - + + if (triBody->getCollisionShape()->isConcave()) { btVector3 rayAabbMin = convexFromLocal.getOrigin(); @@ -349,12 +349,12 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO btCollisionObject* concavebody = triBody; btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - + if (triangleMesh) { triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); } - + if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h index 08ac3f11e20..a6ea33717bc 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h @@ -35,7 +35,7 @@ struct btTriIndex { int m_PartIdTriangleIndex; class btCollisionShape* m_childShape; - + btTriIndex(int partId,int triangleIndex,btCollisionShape* shape) { m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; @@ -75,11 +75,11 @@ class btSoftBodyTriangleCallback : public btTriangleCallback btScalar m_collisionMarginTriangle; btHashMap,btTriIndex> m_shapeCache; - + public: -int m_triangleCount; - -// btPersistentManifold* m_manifoldPtr; + int m_triangleCount; + + // btPersistentManifold* m_manifoldPtr; btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); @@ -88,7 +88,7 @@ int m_triangleCount; virtual ~btSoftBodyTriangleCallback(); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - + void clearCache(); SIMD_FORCE_INLINE const btVector3& getAabbMin() const diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp index d9919967233..6ab93c16402 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -22,57 +22,57 @@ subject to the following restrictions: // static void drawVertex( btIDebugDraw* idraw, - const btVector3& x,btScalar s,const btVector3& c) - { - idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); - idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); - idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); - } + const btVector3& x,btScalar s,const btVector3& c) +{ + idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); + idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); + idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); +} // static void drawBox( btIDebugDraw* idraw, - const btVector3& mins, - const btVector3& maxs, - const btVector3& color) + const btVector3& mins, + const btVector3& maxs, + const btVector3& color) { -const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), - btVector3(maxs.x(),mins.y(),mins.z()), - btVector3(maxs.x(),maxs.y(),mins.z()), - btVector3(mins.x(),maxs.y(),mins.z()), - btVector3(mins.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),maxs.y(),maxs.z()), - btVector3(mins.x(),maxs.y(),maxs.z())}; -idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); -idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); -idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); -idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); -idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); -idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); + const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), + btVector3(maxs.x(),mins.y(),mins.z()), + btVector3(maxs.x(),maxs.y(),mins.z()), + btVector3(mins.x(),maxs.y(),mins.z()), + btVector3(mins.x(),mins.y(),maxs.z()), + btVector3(maxs.x(),mins.y(),maxs.z()), + btVector3(maxs.x(),maxs.y(),maxs.z()), + btVector3(mins.x(),maxs.y(),maxs.z())}; + idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); + idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); + idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); + idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); + idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); + idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); } // static void drawTree( btIDebugDraw* idraw, - const btDbvtNode* node, - int depth, - const btVector3& ncolor, - const btVector3& lcolor, - int mindepth, - int maxdepth) + const btDbvtNode* node, + int depth, + const btVector3& ncolor, + const btVector3& lcolor, + int mindepth, + int maxdepth) { -if(node) + if(node) { - if(node->isinternal()&&((depthisinternal()&&((depthchilds[0],depth+1,ncolor,lcolor,mindepth,maxdepth); - drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); + drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); + drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); } - if(depth>=mindepth) + if(depth>=mindepth) { - const btScalar scl=(btScalar)(node->isinternal()?1:1); - const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; - const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; - drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); + const btScalar scl=(btScalar)(node->isinternal()?1:1); + const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; + const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; + drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); } } } @@ -81,25 +81,25 @@ if(node) template static inline T sum(const btAlignedObjectArray& items) { -T v; -if(items.size()) + T v; + if(items.size()) { - v=items[0]; - for(int i=1,ni=items.size();i static inline void add(btAlignedObjectArray& items,const Q& value) { -for(int i=0,ni=items.size();i static inline void mul(btAlignedObjectArray& items,const Q& value) { -for(int i=0,ni=items.size();i static inline T average(const btAlignedObjectArray& items) { -const btScalar n=(btScalar)(items.size()>0?items.size():1); -return(sum(items)/n); + const btScalar n=(btScalar)(items.size()>0?items.size():1); + return(sum(items)/n); } // @@ -136,27 +136,27 @@ static inline btScalar tetravolume(const btVector3& x0, // #if 0 static btVector3 stresscolor(btScalar stress) - { +{ static const btVector3 spectrum[]= { btVector3(1,0,1), - btVector3(0,0,1), - btVector3(0,1,1), - btVector3(0,1,0), - btVector3(1,1,0), - btVector3(1,0,0), - btVector3(1,0,0)}; + btVector3(0,0,1), + btVector3(0,1,1), + btVector3(0,1,0), + btVector3(1,1,0), + btVector3(1,0,0), + btVector3(1,0,0)}; static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; static const btScalar one=1; stress=btMax(0,btMin(1,stress))*ncolors; const int sel=(int)stress; const btScalar frc=stress-sel; return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); - } +} #endif // void btSoftBodyHelpers::Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) + btIDebugDraw* idraw, + int drawflags) { const btScalar scl=(btScalar)0.1; const btScalar nscl=scl*5; @@ -251,29 +251,29 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; const btVector3 c=(x[0]+x[1]+x[2])/3; idraw->drawTriangle((x[0]-c)*scl+c, - (x[1]-c)*scl+c, - (x[2]-c)*scl+c, - col,alp); + (x[1]-c)*scl+c, + (x[2]-c)*scl+c, + col,alp); } } /* Clusters */ if(0!=(drawflags&fDrawFlags::Clusters)) - { + { srand(1806); for(i=0;im_clusters.size();++i) - { + { if(psb->m_clusters[i]->m_collide) - { + { btVector3 color( rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX); + rand()/(btScalar)RAND_MAX, + rand()/(btScalar)RAND_MAX); color=color.normalized()*0.75; btAlignedObjectArray vertices; vertices.resize(psb->m_clusters[i]->m_nodes.size()); for(j=0,nj=vertices.size();jm_clusters[i]->m_nodes[j]->m_x; - } + } HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); HullResult hres; HullLibrary hlib; @@ -284,32 +284,32 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, mul(hres.m_OutputVertices,(btScalar)1); add(hres.m_OutputVertices,center); for(j=0;j<(int)hres.mNumFaces;++j) - { + { const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; idraw->drawTriangle(hres.m_OutputVertices[idx[0]], - hres.m_OutputVertices[idx[1]], - hres.m_OutputVertices[idx[2]], - color,1); - } - hlib.ReleaseResult(hres); + hres.m_OutputVertices[idx[1]], + hres.m_OutputVertices[idx[2]], + color,1); } + hlib.ReleaseResult(hres); + } /* Velocities */ - #if 0 +#if 0 for(int j=0;jm_clusters[i].m_nodes.size();++j) - { + { const btSoftBody::Cluster& c=psb->m_clusters[i]; const btVector3 r=c.m_nodes[j]->m_x-c.m_com; const btVector3 v=c.m_lv+cross(c.m_av,r); idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); - } - #endif + } +#endif /* Frame */ btSoftBody::Cluster& c=*psb->m_clusters[i]; idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); - } } + } /* Notes */ if(0!=(drawflags&fDrawFlags::Notes)) { @@ -318,9 +318,9 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, const btSoftBody::Note& n=psb->m_notes[i]; btVector3 p=n.m_offset; for(int j=0;jm_x*n.m_coords[j]; - } + } idraw->draw3dText(p,n.m_text); } } @@ -333,33 +333,33 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Joints */ if(0!=(drawflags&fDrawFlags::Joints)) { - for(i=0;im_joints.size();++i) + for(i=0;im_joints.size();++i) { - const btSoftBody::Joint* pj=psb->m_joints[i]; - switch(pj->Type()) + const btSoftBody::Joint* pj=psb->m_joints[i]; + switch(pj->Type()) { case btSoftBody::Joint::eType::Linear: { - const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; - const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; - idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); - idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); - drawVertex(idraw,a0,0.25,btVector3(1,1,0)); - drawVertex(idraw,a1,0.25,btVector3(0,1,1)); + const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; + const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; + const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; + idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); + idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); + drawVertex(idraw,a0,0.25,btVector3(1,1,0)); + drawVertex(idraw,a1,0.25,btVector3(0,1,1)); } - break; + break; case btSoftBody::Joint::eType::Angular: { - const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; - const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); - const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); - const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; - idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); - idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); - idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); - idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); + const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; + const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); + const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); + const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; + const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; + idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); + idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); + idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); + idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); } } } @@ -368,10 +368,10 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, // void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool /*stress*/) + btIDebugDraw* idraw, + bool masses, + bool areas, + bool /*stress*/) { for(int i=0;im_nodes.size();++i) { @@ -394,34 +394,34 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, // void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { -drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); + drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { -drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); + drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { -drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); + drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) + btIDebugDraw* idraw) { if(psb->m_pose.m_bframe) { @@ -445,9 +445,9 @@ void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, // btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, - const btVector3& to, - int res, - int fixeds) + const btVector3& to, + int res, + int fixeds) { /* Create nodes */ const int r=res+2; @@ -477,13 +477,13 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, cons // btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags) + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags) { #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) /* Create nodes */ @@ -552,10 +552,216 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const return(psb); } +// +btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags, + float* tex_coords) +{ + + /* + * + * corners: + * + * [0][0] corner00 ------- corner01 [resx][0] + * | | + * | | + * [0][resy] corner10 -------- corner11 [resx][resy] + * + * + * + * + * + * + * "fixedgs" map: + * + * corner00 --> +1 + * corner01 --> +2 + * corner10 --> +4 + * corner11 --> +8 + * upper middle --> +16 + * left middle --> +32 + * right middle --> +64 + * lower middle --> +128 + * center --> +256 + * + * + * tex_coords size (resx-1)*(resy-1)*12 + * + * + * + * SINGLE QUAD INTERNALS + * + * 1) btSoftBody's nodes and links, + * diagonal link is optional ("gendiags") + * + * + * node00 ------ node01 + * | . + * | . + * | . + * | . + * | . + * node10 node11 + * + * + * + * 2) Faces: + * two triangles, + * UV Coordinates (hier example for single quad) + * + * (0,1) (0,1) (1,1) + * 1 |\ 3 \-----| 2 + * | \ \ | + * | \ \ | + * | \ \ | + * | \ \ | + * 2 |-----\ 3 \| 1 + * (0,0) (1,0) (1,0) + * + * + * + * + * + * + */ + +#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) + /* Create nodes */ + if((resx<2)||(resy<2)) return(0); + const int rx=resx; + const int ry=resy; + const int tot=rx*ry; + btVector3* x=new btVector3[tot]; + btScalar* m=new btScalar[tot]; + + int iy; + + for(iy=0;iysetMass(IDX(0,0),0); + if(fixeds&2) psb->setMass(IDX(rx-1,0),0); + if(fixeds&4) psb->setMass(IDX(0,ry-1),0); + if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); + if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0); + if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0); + if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0); + if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0); + if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0); + delete[] x; + delete[] m; + + + int z = 0; + /* Create links and faces */ + for(iy=0;iyappendLink(node00,node01); + if(mdy) psb->appendLink(node00,node10); + if(mdx&&mdy) + { + psb->appendFace(node00,node10,node11); + if (tex_coords) { + tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); + tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); + tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); + tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); + tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); + tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); + } + psb->appendFace(node11,node01,node00); + if (tex_coords) { + tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); + tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); + tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); + tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); + tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); + tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); + } + if (gendiags) psb->appendLink(node00,node11); + z += 12; + } + } + } + /* Finished */ +#undef IDX + return(psb); +} + +float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) +{ + + /* + * + * + * node00 --- node01 + * | | + * node10 --- node11 + * + * + * ID map: + * + * node00 s --> 0 + * node00 t --> 1 + * + * node01 s --> 3 + * node01 t --> 1 + * + * node10 s --> 0 + * node10 t --> 2 + * + * node11 s --> 3 + * node11 t --> 2 + * + * + */ + + float tc=0.0f; + if (id == 0) { + tc = (1.0f/((resx-1))*ix); + } + else if (id==1) { + tc = (1.0f/((resy-1))*(resy-1-iy)); + } + else if (id==2) { + tc = (1.0f/((resy-1))*(resy-1-iy-1)); + } + else if (id==3) { + tc = (1.0f/((resx-1))*(ix+1)); + } + return tc; +} // btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, - const btVector3& radius, - int res) + const btVector3& radius, + int res) { struct Hammersley { @@ -586,8 +792,8 @@ btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,c // btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, - const int* triangles, - int ntriangles) + const int* triangles, + int ntriangles) { int maxidx=0; int i,j,ni; @@ -615,7 +821,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo if(!chks[IDX(idx[j],idx[k])]) { chks[IDX(idx[j],idx[k])]=true; - chks[IDX(idx[k],idx[k])]=true; + chks[IDX(idx[k],idx[j])]=true; psb->appendLink(idx[j],idx[k]); } } @@ -628,7 +834,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo // btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, - int nvertices) + int nvertices) { HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); HullResult hres; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h index e9c6cb20657..0e3b50397ee 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h @@ -39,69 +39,81 @@ struct fDrawFlags { enum _ { Joints = 0x1000, /* presets */ Std = Links+Faces+Tetras+Anchors+Notes+Joints, - StdTetra = Std-Faces+Tetras, + StdTetra = Std-Faces+Tetras };}; struct btSoftBodyHelpers { /* Draw body */ static void Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags=fDrawFlags::Std); + btIDebugDraw* idraw, + int drawflags=fDrawFlags::Std); /* Draw body infos */ static void DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool stress); + btIDebugDraw* idraw, + bool masses, + bool areas, + bool stress); /* Draw node tree */ static void DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); /* Draw face tree */ static void DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); /* Draw cluster tree */ static void DrawClusterTree(btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); /* Draw rigid frame */ static void DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw); + btIDebugDraw* idraw); /* Create a rope */ static btSoftBody* CreateRope( btSoftBodyWorldInfo& worldInfo, - const btVector3& from, - const btVector3& to, - int res, - int fixeds); + const btVector3& from, + const btVector3& to, + int res, + int fixeds); /* Create a patch */ static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags); + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags); + /* Create a patch with UV Texture Coordinates */ + static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags, + float* tex_coords=0); + static float CalculateUV(int resx,int resy,int ix,int iy,int id); /* Create an ellipsoid */ static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, - const btVector3& center, - const btVector3& radius, - int res); + const btVector3& center, + const btVector3& radius, + int res); /* Create from trimesh */ static btSoftBody* CreateFromTriMesh( btSoftBodyWorldInfo& worldInfo, - const btScalar* vertices, - const int* triangles, - int ntriangles); + const btScalar* vertices, + const int* triangles, + int ntriangles); /* Create from convex-hull */ static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo, - const btVector3* vertices, - int nvertices); + const btVector3* vertices, + int nvertices); }; #endif //SOFT_BODY_HELPERS_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h index 8fcf633fecc..5f0f7d54318 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h @@ -31,14 +31,14 @@ subject to the following restrictions: template struct btSymMatrix { - btSymMatrix() : dim(0) {} - btSymMatrix(int n,const T& init=T()) { resize(n,init); } -void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } -int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r store; -int dim; + btSymMatrix() : dim(0) {} + btSymMatrix(int n,const T& init=T()) { resize(n,init); } + void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } + int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r store; + int dim; }; // @@ -48,10 +48,11 @@ class btSoftBodyCollisionShape : public btConcaveShape { public: btSoftBody* m_body; - + btSoftBodyCollisionShape(btSoftBody* backptr) { - m_body=backptr; + m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; + m_body=backptr; } virtual ~btSoftBodyCollisionShape() @@ -68,37 +69,34 @@ public: ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { - /* t should be identity, but better be safe than...fast? */ - const btVector3 mins=m_body->m_bounds[0]; - const btVector3 maxs=m_body->m_bounds[1]; - const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),maxs.y(),maxs.z()), - t*btVector3(mins.x(),maxs.y(),maxs.z())}; - aabbMin=aabbMax=crns[0]; - for(int i=1;i<8;++i) + /* t should be identity, but better be safe than...fast? */ + const btVector3 mins=m_body->m_bounds[0]; + const btVector3 maxs=m_body->m_bounds[1]; + const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), + t*btVector3(maxs.x(),mins.y(),mins.z()), + t*btVector3(maxs.x(),maxs.y(),mins.z()), + t*btVector3(mins.x(),maxs.y(),mins.z()), + t*btVector3(mins.x(),mins.y(),maxs.z()), + t*btVector3(maxs.x(),mins.y(),maxs.z()), + t*btVector3(maxs.x(),maxs.y(),maxs.z()), + t*btVector3(mins.x(),maxs.y(),maxs.z())}; + aabbMin=aabbMax=crns[0]; + for(int i=1;i<8;++i) { - aabbMin.setMin(crns[i]); - aabbMax.setMax(crns[i]); + aabbMin.setMin(crns[i]); + aabbMax.setMax(crns[i]); } } - virtual int getShapeType() const - { - return SOFTBODY_SHAPE_PROXYTYPE; - } + virtual void setLocalScaling(const btVector3& /*scaling*/) { ///na } virtual const btVector3& getLocalScaling() const { - static const btVector3 dummy(1,1,1); - return dummy; + static const btVector3 dummy(1,1,1); + return dummy; } virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const { @@ -121,24 +119,24 @@ public: const btSoftBody::Cluster* m_cluster; btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } - - + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const - { + { btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; btScalar d=dot(vec,n[0]->m_x); int j=0; for(int i=1,ni=m_cluster->m_nodes.size();im_x); if(k>d) { d=k;j=i; } - } - return(n[j]->m_x); } + return(n[j]->m_x); + } virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const - { + { return(localGetSupportingVertex(vec)); - } + } //notice that the vectors should be unit length virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const {} @@ -151,7 +149,7 @@ public: {} virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } - + //debugging virtual const char* getName()const {return "SOFTCLUSTER";} @@ -173,8 +171,8 @@ public: template static inline void ZeroInitialize(T& value) { -static const T zerodummy; -value=zerodummy; + static const T zerodummy; + value=zerodummy; } // template @@ -194,23 +192,23 @@ static inline T InvLerp(const T& a,const T& b,btScalar t) { return((b+a*t-b*t)/(a*b)); } // static inline btMatrix3x3 Lerp( const btMatrix3x3& a, - const btMatrix3x3& b, - btScalar t) -{ -btMatrix3x3 r; -r[0]=Lerp(a[0],b[0],t); -r[1]=Lerp(a[1],b[1],t); -r[2]=Lerp(a[2],b[2],t); -return(r); + const btMatrix3x3& b, + btScalar t) +{ + btMatrix3x3 r; + r[0]=Lerp(a[0],b[0],t); + r[1]=Lerp(a[1],b[1],t); + r[2]=Lerp(a[2],b[2],t); + return(r); } // static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) { -const btScalar sql=v.length2(); -if(sql>(maxlength*maxlength)) - return((v*maxlength)/btSqrt(sql)); + const btScalar sql=v.length2(); + if(sql>(maxlength*maxlength)) + return((v*maxlength)/btSqrt(sql)); else - return(v); + return(v); } // template @@ -235,8 +233,8 @@ static inline bool SameSign(const T& x,const T& y) // static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) { -const btVector3 d=x-y; -return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); + const btVector3 d=x-y; + return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); } // static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) @@ -273,7 +271,7 @@ static inline btMatrix3x3 Diagonal(btScalar x) } // static inline btMatrix3x3 Add(const btMatrix3x3& a, - const btMatrix3x3& b) + const btMatrix3x3& b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]+b[i]; @@ -281,7 +279,7 @@ static inline btMatrix3x3 Add(const btMatrix3x3& a, } // static inline btMatrix3x3 Sub(const btMatrix3x3& a, - const btMatrix3x3& b) + const btMatrix3x3& b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]-b[i]; @@ -289,7 +287,7 @@ static inline btMatrix3x3 Sub(const btMatrix3x3& a, } // static inline btMatrix3x3 Mul(const btMatrix3x3& a, - btScalar b) + btScalar b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]*b; @@ -298,9 +296,9 @@ static inline btMatrix3x3 Mul(const btMatrix3x3& a, // static inline void Orthogonalize(btMatrix3x3& m) { -m[2]=cross(m[0],m[1]).normalized(); -m[1]=cross(m[2],m[0]).normalized(); -m[0]=cross(m[1],m[2]).normalized(); + m[2]=cross(m[0],m[1]).normalized(); + m[1]=cross(m[2],m[0]).normalized(); + m[0]=cross(m[1],m[2]).normalized(); } // static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) @@ -311,90 +309,90 @@ static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const bt // static inline btMatrix3x3 ImpulseMatrix( btScalar dt, - btScalar ima, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) + btScalar ima, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) { return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); } // static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, - btScalar imb,const btMatrix3x3& iib,const btVector3& rb) + btScalar imb,const btMatrix3x3& iib,const btVector3& rb) { -return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); + return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); } // static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, - const btMatrix3x3& iib) + const btMatrix3x3& iib) { -return(Add(iia,iib).inverse()); + return(Add(iia,iib).inverse()); } // static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) + const btVector3& a) { return(a*dot(v,a)); } // static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) + const btVector3& a) { return(v-ProjectOnAxis(v,a)); } // static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - btVector3& prj, - btScalar& sqd) + const btVector3& b, + btVector3& prj, + btScalar& sqd) { -const btVector3 d=b-a; -const btScalar m2=d.length2(); -if(m2>SIMD_EPSILON) + const btVector3 d=b-a; + const btScalar m2=d.length2(); + if(m2>SIMD_EPSILON) { - const btScalar t=Clamp(-dot(a,d)/m2,0,1); - const btVector3 p=a+d*t; - const btScalar l2=p.length2(); - if(l2(-dot(a,d)/m2,0,1); + const btVector3 p=a+d*t; + const btScalar l2=p.length2(); + if(l2SIMD_EPSILON) + const btVector3& b, + const btVector3& c, + btVector3& prj, + btScalar& sqd) +{ + const btVector3& q=cross(b-a,c-a); + const btScalar m2=q.length2(); + if(m2>SIMD_EPSILON) { - const btVector3 n=q/btSqrt(m2); - const btScalar k=dot(a,n); - const btScalar k2=k*k; - if(k20)&& - (dot(cross(b-p,c-p),q)>0)&& - (dot(cross(c-p,a-p),q)>0)) + const btVector3 p=n*k; + if( (dot(cross(a-p,b-p),q)>0)&& + (dot(cross(b-p,c-p),q)>0)&& + (dot(cross(c-p,a-p),q)>0)) { - prj=p; - sqd=k2; + prj=p; + sqd=k2; } else { - ProjectOrigin(a,b,prj,sqd); - ProjectOrigin(b,c,prj,sqd); - ProjectOrigin(c,a,prj,sqd); + ProjectOrigin(a,b,prj,sqd); + ProjectOrigin(b,c,prj,sqd); + ProjectOrigin(c,a,prj,sqd); } } } @@ -403,53 +401,53 @@ if(m2>SIMD_EPSILON) // template static inline T BaryEval( const T& a, - const T& b, - const T& c, - const btVector3& coord) + const T& b, + const T& c, + const btVector3& coord) { return(a*coord.x()+b*coord.y()+c*coord.z()); } // static inline btVector3 BaryCoord( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& p) -{ -const btScalar w[]={ cross(a-p,b-p).length(), - cross(b-p,c-p).length(), - cross(c-p,a-p).length()}; -const btScalar isum=1/(w[0]+w[1]+w[2]); -return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); + const btVector3& b, + const btVector3& c, + const btVector3& p) +{ + const btScalar w[]={ cross(a-p,b-p).length(), + cross(b-p,c-p).length(), + cross(c-p,a-p).length()}; + const btScalar isum=1/(w[0]+w[1]+w[2]); + return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); } // static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, - const btVector3& a, - const btVector3& b, - const btScalar accuracy, - const int maxiterations=256) -{ -btScalar span[2]={0,1}; -btScalar values[2]={fn->Eval(a),fn->Eval(b)}; -if(values[0]>values[1]) + const btVector3& a, + const btVector3& b, + const btScalar accuracy, + const int maxiterations=256) +{ + btScalar span[2]={0,1}; + btScalar values[2]={fn->Eval(a),fn->Eval(b)}; + if(values[0]>values[1]) { - btSwap(span[0],span[1]); - btSwap(values[0],values[1]); + btSwap(span[0],span[1]); + btSwap(values[0],values[1]); } -if(values[0]>-accuracy) return(-1); -if(values[1]<+accuracy) return(-1); -for(int i=0;i-accuracy) return(-1); + if(values[1]<+accuracy) return(-1); + for(int i=0;iEval(Lerp(a,b,t)); - if((t<=0)||(t>=1)) break; - if(btFabs(v)Eval(Lerp(a,b,t)); + if((t<=0)||(t>=1)) break; + if(btFabs(v)m_x, - &f.m_n[1]->m_x, - &f.m_n[2]->m_x}; -btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); -vol.Expand(btVector3(margin,margin,margin)); -return(vol); + btScalar margin) +{ + const btVector3* pts[]={ &f.m_n[0]->m_x, + &f.m_n[1]->m_x, + &f.m_n[2]->m_x}; + btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); + vol.Expand(btVector3(margin,margin,margin)); + return(vol); } // static inline btVector3 CenterOf( const btSoftBody::Face& f) { -return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); + return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); } // static inline btScalar AreaOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2) + const btVector3& x1, + const btVector3& x2) { const btVector3 a=x1-x0; const btVector3 b=x2-x0; @@ -494,9 +492,9 @@ static inline btScalar AreaOf( const btVector3& x0, // static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) { const btVector3 a=x1-x0; const btVector3 b=x2-x0; @@ -506,8 +504,8 @@ static inline btScalar VolumeOf( const btVector3& x0, // static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, - const btVector3& x, - btSoftBody::sMedium& medium) + const btVector3& x, + btSoftBody::sMedium& medium) { medium.m_velocity = btVector3(0,0,0); medium.m_pressure = 0; @@ -525,8 +523,8 @@ static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, // static inline void ApplyClampedForce( btSoftBody::Node& n, - const btVector3& f, - btScalar dt) + const btVector3& f, + btScalar dt) { const btScalar dtim=dt*n.m_im; if((f*dtim).length2()>n.m_v.length2()) @@ -541,13 +539,13 @@ static inline void ApplyClampedForce( btSoftBody::Node& n, // static inline int MatchEdge( const btSoftBody::Node* a, - const btSoftBody::Node* b, - const btSoftBody::Node* ma, - const btSoftBody::Node* mb) + const btSoftBody::Node* b, + const btSoftBody::Node* ma, + const btSoftBody::Node* mb) { -if((a==ma)&&(b==mb)) return(0); -if((a==mb)&&(b==ma)) return(1); -return(-1); + if((a==ma)&&(b==mb)) return(0); + if((a==mb)&&(b==ma)) return(1); + return(-1); } // @@ -557,56 +555,56 @@ return(-1); // struct btEigen { -static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) + static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) { - static const int maxiterations=16; - static const btScalar accuracy=(btScalar)0.0001; - btMatrix3x3& v=*vectors; - int iterations=0; - vectors->setIdentity(); - do { - int p=0,q=1; - if(btFabs(a[p][q])accuracy) + static const int maxiterations=16; + static const btScalar accuracy=(btScalar)0.0001; + btMatrix3x3& v=*vectors; + int iterations=0; + vectors->setIdentity(); + do { + int p=0,q=1; + if(btFabs(a[p][q])accuracy) { - const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); - const btScalar z=btFabs(w); - const btScalar t=w/(z*(btSqrt(1+w*w)+z)); - if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ + const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); + const btScalar z=btFabs(w); + const btScalar t=w/(z*(btSqrt(1+w*w)+z)); + if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ { - const btScalar c=1/btSqrt(t*t+1); - const btScalar s=c*t; - mulPQ(a,c,s,p,q); - mulTPQ(a,c,s,p,q); - mulPQ(v,c,s,p,q); + const btScalar c=1/btSqrt(t*t+1); + const btScalar s=c*t; + mulPQ(a,c,s,p,q); + mulTPQ(a,c,s,p,q); + mulPQ(v,c,s,p,q); } else break; } else break; } while((++iterations)data; - btSoftClusterCollisionShape cshape(cluster); - const btConvexShape* rshape=(const btConvexShape*)prb->getCollisionShape(); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), - rshape,prb->getInterpolationWorldTransform(), - btVector3(1,0,0),res)) + btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; + btSoftClusterCollisionShape cshape(cluster); + const btConvexShape* rshape=(const btConvexShape*)m_colObj->getCollisionShape(); + btGjkEpaSolver2::sResults res; + if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), + rshape,m_colObj->getInterpolationWorldTransform(), + btVector3(1,0,0),res)) { - btSoftBody::CJoint joint; - if(SolveContact(res,cluster,prb,joint)) + btSoftBody::CJoint joint; + if(SolveContact(res,cluster,m_colObj,joint))//prb,joint)) { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;psb->m_joints.push_back(pj); - if(prb->isStaticOrKinematicObject()) + btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); + *pj=joint;psb->m_joints.push_back(pj); + if(m_colObj->isStaticOrKinematicObject()) { - pj->m_erp *= psb->m_cfg.kSKHR_CL; - pj->m_split *= psb->m_cfg.kSK_SPLT_CL; + pj->m_erp *= psb->m_cfg.kSKHR_CL; + pj->m_split *= psb->m_cfg.kSK_SPLT_CL; } else { - pj->m_erp *= psb->m_cfg.kSRHR_CL; - pj->m_split *= psb->m_cfg.kSR_SPLT_CL; + pj->m_erp *= psb->m_cfg.kSRHR_CL; + pj->m_split *= psb->m_cfg.kSR_SPLT_CL; } } } } - void Process(btSoftBody* ps,btRigidBody* pr) + void Process(btSoftBody* ps,btCollisionObject* colOb) { - psb = ps; - prb = pr; - idt = ps->m_sst.isdt; - margin = ps->getCollisionShape()->getMargin()+ - pr->getCollisionShape()->getMargin(); - friction = btMin(psb->m_cfg.kDF,prb->getFriction()); - btVector3 mins; - btVector3 maxs; - - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(1,1,1)*margin); - btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this); + psb = ps; + m_colObj = colOb; + idt = ps->m_sst.isdt; + margin = m_colObj->getCollisionShape()->getMargin(); + ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful. + friction = btMin(psb->m_cfg.kDF,m_colObj->getFriction()); + btVector3 mins; + btVector3 maxs; + + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; + colOb->getCollisionShape()->getAabb(colOb->getInterpolationWorldTransform(),mins,maxs); + volume=btDbvtVolume::FromMM(mins,maxs); + volume.Expand(btVector3(1,1,1)*margin); + ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this); } }; // @@ -762,36 +761,53 @@ struct btSoftColliders // struct CollideCL_SS : ClusterBase { - btSoftBody* bodies[2]; - void Process(const btDbvtNode* la,const btDbvtNode* lb) + btSoftBody* bodies[2]; + void Process(const btDbvtNode* la,const btDbvtNode* lb) { - btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; - btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; - btSoftClusterCollisionShape csa(cla); - btSoftClusterCollisionShape csb(clb); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), - &csb,btTransform::getIdentity(), - cla->m_com-clb->m_com,res)) + btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; + btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; + + + bool connected=false; + if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size())) { - btSoftBody::CJoint joint; - if(SolveContact(res,cla,clb,joint)) + connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex]; + } + + if (!connected) + { + btSoftClusterCollisionShape csa(cla); + btSoftClusterCollisionShape csb(clb); + btGjkEpaSolver2::sResults res; + if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), + &csb,btTransform::getIdentity(), + cla->m_com-clb->m_com,res)) { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;bodies[0]->m_joints.push_back(pj); - pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); - pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; + btSoftBody::CJoint joint; + if(SolveContact(res,cla,clb,joint)) + { + btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); + *pj=joint;bodies[0]->m_joints.push_back(pj); + pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); + pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; + } } + } else + { + static int count=0; + count++; + //printf("count=%d\n",count); + } } - void Process(btSoftBody* psa,btSoftBody* psb) + void Process(btSoftBody* psa,btSoftBody* psb) { - idt = psa->m_sst.isdt; - margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; - friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); - bodies[0] = psa; - bodies[1] = psb; - btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); + idt = psa->m_sst.isdt; + margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; + friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); + bodies[0] = psa; + bodies[1] = psb; + psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); } }; // @@ -799,96 +815,99 @@ struct btSoftColliders // struct CollideSDF_RS : btDbvt::ICollide { - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; - DoNode(*node); + btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; + DoNode(*node); } - void DoNode(btSoftBody::Node& n) const + void DoNode(btSoftBody::Node& n) const { - const btScalar m=n.m_im>0?dynmargin:stamargin; - btSoftBody::RContact c; - if( (!n.m_battach)&& - psb->checkContact(prb,n.m_x,m,c.m_cti)) + const btScalar m=n.m_im>0?dynmargin:stamargin; + btSoftBody::RContact c; + if( (!n.m_battach)&& + psb->checkContact(m_colObj1,n.m_x,m,c.m_cti)) { - const btScalar ima=n.m_im; - const btScalar imb=prb->getInvMass(); - const btScalar ms=ima+imb; - if(ms>0) + const btScalar ima=n.m_im; + const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f; + const btScalar ms=ima+imb; + if(ms>0) { - const btTransform& wtr=prb->getInterpolationWorldTransform(); - const btMatrix3x3& iwi=prb->getInvInertiaTensorWorld(); - const btVector3 ra=n.m_x-wtr.getOrigin(); - const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt; - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=vb-va; - const btScalar dn=dot(vr,c.m_cti.m_normal); - const btVector3 fv=vr-c.m_cti.m_normal*dn; - const btScalar fc=psb->m_cfg.kDF*prb->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); - c.m_c1 = ra; - c.m_c2 = ima*psb->m_sst.sdt; - c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; - c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - prb->activate(); + const btTransform& wtr=m_rigidBody?m_rigidBody->getInterpolationWorldTransform() : m_colObj1->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0); + const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra=n.m_x-wtr.getOrigin(); + const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0); + const btVector3 vb=n.m_x-n.m_q; + const btVector3 vr=vb-va; + const btScalar dn=dot(vr,c.m_cti.m_normal); + const btVector3 fv=vr-c.m_cti.m_normal*dn; + const btScalar fc=psb->m_cfg.kDF*m_colObj1->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); + c.m_c1 = ra; + c.m_c2 = ima*psb->m_sst.sdt; + c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; + c.m_c4 = m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); } } } - btSoftBody* psb; - btRigidBody* prb; - btScalar dynmargin; - btScalar stamargin; + btSoftBody* psb; + btCollisionObject* m_colObj1; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; }; // // CollideVF_SS // struct CollideVF_SS : btDbvt::ICollide { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) { - btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; - btSoftBody::Face* face=(btSoftBody::Face*)lface->data; - btVector3 o=node->m_x; - btVector3 p; - btScalar d=SIMD_INFINITY; - ProjectOrigin( face->m_n[0]->m_x-o, - face->m_n[1]->m_x-o, - face->m_n[2]->m_x-o, - p,d); - const btScalar m=mrg+(o-node->m_q).length()*2; - if(d<(m*m)) + btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; + btSoftBody::Face* face=(btSoftBody::Face*)lface->data; + btVector3 o=node->m_x; + btVector3 p; + btScalar d=SIMD_INFINITY; + ProjectOrigin( face->m_n[0]->m_x-o, + face->m_n[1]->m_x-o, + face->m_n[2]->m_x-o, + p,d); + const btScalar m=mrg+(o-node->m_q).length()*2; + if(d<(m*m)) { - const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; - const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); - const btScalar ma=node->m_im; - btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); - if( (n[0]->m_im<=0)|| - (n[1]->m_im<=0)|| - (n[2]->m_im<=0)) + const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; + const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); + const btScalar ma=node->m_im; + btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); + if( (n[0]->m_im<=0)|| + (n[1]->m_im<=0)|| + (n[2]->m_im<=0)) { - mb=0; + mb=0; } - const btScalar ms=ma+mb; - if(ms>0) + const btScalar ms=ma+mb; + if(ms>0) { - btSoftBody::SContact c; - c.m_normal = p/-btSqrt(d); - c.m_margin = m; - c.m_node = node; - c.m_face = face; - c.m_weights = w; - c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); - c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; - c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; - psb[0]->m_scontacts.push_back(c); + btSoftBody::SContact c; + c.m_normal = p/-btSqrt(d); + c.m_margin = m; + c.m_node = node; + c.m_face = face; + c.m_weights = w; + c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); + c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; + c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; + psb[0]->m_scontacts.push_back(c); } } } - btSoftBody* psb[2]; - btScalar mrg; + btSoftBody* psb[2]; + btScalar mrg; }; }; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp index 53ac2782583..f5a67f6d895 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp @@ -29,10 +29,10 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; m_swappedSoftRigidConvexCreateFunc->m_swapped=true; @@ -40,26 +40,27 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; m_swappedSoftRigidConcaveCreateFunc->m_swapped=true; #endif //replace pool by a new one, with potential larger size - + if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool) { int curElemSize = m_collisionAlgorithmPool->getElementSize(); ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool - - + + int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm); int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm); int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm); int collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); + if (collisionAlgorithmMaxElementSize > curElemSize) { m_collisionAlgorithmPool->~btPoolAllocator(); @@ -69,9 +70,6 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura } } - - - } btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration() @@ -93,7 +91,7 @@ btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfigur btAlignedFree( m_swappedSoftRigidConcaveCreateFunc); #endif } - + ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) { diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h index 41c3af96939..21addcfe2e1 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h @@ -32,7 +32,7 @@ class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfi btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConvexCreateFunc; btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc; btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc; - + public: btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index e5feb5ef749..11ad9e7daba 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -35,13 +35,13 @@ m_isSwapped(isSwapped) btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() { - + //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject); /*if (m_ownManifold) { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); } */ diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h index 74327e6c635..adc3844e363 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h @@ -28,15 +28,15 @@ class btSoftBody; /// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm { -// bool m_ownManifold; -// btPersistentManifold* m_manifoldPtr; + // bool m_ownManifold; + // btPersistentManifold* m_manifoldPtr; btSoftBody* m_softBody; btCollisionObject* m_rigidCollisionObject; ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean bool m_isSwapped; - + public: btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); @@ -52,7 +52,7 @@ public: //we don't add any manifolds } - + struct CreateFunc :public btCollisionAlgorithmCreateFunc { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index b363a2efbc1..a0069b95145 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -28,17 +28,17 @@ subject to the following restrictions: btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) { -m_drawFlags = fDrawFlags::Std; -m_drawNodeTree = true; -m_drawFaceTree = false; -m_drawClusterTree = false; -m_sbi.m_broadphase = pairCache; -m_sbi.m_dispatcher = dispatcher; -m_sbi.m_sparsesdf.Initialize(); -m_sbi.m_sparsesdf.Reset(); + m_drawFlags = fDrawFlags::Std; + m_drawNodeTree = true; + m_drawFaceTree = false; + m_drawClusterTree = false; + m_sbi.m_broadphase = pairCache; + m_sbi.m_dispatcher = dispatcher; + m_sbi.m_sparsesdf.Initialize(); + m_sbi.m_sparsesdf.Reset(); } - + btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() { @@ -55,7 +55,7 @@ void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) psb->predictMotion(timeStep); } } - + void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) { btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); @@ -63,6 +63,13 @@ void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) ///solve soft bodies constraints solveSoftBodiesConstraints(); + //self collisions + for ( int i=0;idefaultCollisionHandler(psb); + } + ///update soft bodies updateSoftBodies(); @@ -71,7 +78,7 @@ void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) void btSoftRigidDynamicsWorld::updateSoftBodies() { BT_PROFILE("updateSoftBodies"); - + for ( int i=0;i btSoftBodyArray; class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld { - + btSoftBodyArray m_softBodies; int m_drawFlags; bool m_drawNodeTree; @@ -32,9 +32,9 @@ class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld btSoftBodyWorldInfo m_sbi; protected: - + virtual void predictUnconstraintMotion(btScalar timeStep); - + virtual void internalSingleStepSimulation( btScalar timeStep); void updateSoftBodies(); @@ -43,17 +43,17 @@ protected: public: - + btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); virtual ~btSoftRigidDynamicsWorld(); - + virtual void debugDrawWorld(); - + void addSoftBody(btSoftBody* body); void removeSoftBody(btSoftBody* body); - + int getDrawFlags() const { return(m_drawFlags); } void setDrawFlags(int f) { m_drawFlags=f; } @@ -66,7 +66,7 @@ public: return m_sbi; } - + btSoftBodyArray& getSoftBodyArray() { return m_softBodies; @@ -76,7 +76,7 @@ public: { return m_softBodies; } - + }; #endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h index 7ca9c3415c9..1b34e0af60f 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h @@ -33,7 +33,7 @@ class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm btSoftBody* m_softBody0; btSoftBody* m_softBody1; - + public: btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btCollisionAlgorithm(ci) {} diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h index eafe74be1ae..cc4266732ae 100644 --- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h +++ b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h @@ -23,139 +23,139 @@ subject to the following restrictions: // Modified Paul Hsieh hash template unsigned int HsiehHash(const void* pdata) - { +{ const unsigned short* data=(const unsigned short*)pdata; unsigned hash=DWORDLEN<<2,tmp; for(int i=0;i>11; - } + } hash^=hash<<3;hash+=hash>>5; hash^=hash<<4;hash+=hash>>17; hash^=hash<<25;hash+=hash>>6; return(hash); - } +} template struct btSparseSdf - { +{ // // Inner types // struct IntFrac - { + { int b; int i; btScalar f; - }; + }; struct Cell - { + { btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; int c[3]; int puid; unsigned hash; btCollisionShape* pclient; Cell* next; - }; + }; // // Fields // - + btAlignedObjectArray cells; btScalar voxelsz; int puid; int ncells; int nprobes; int nqueries; - + // // Methods // - + // void Initialize(int hashsize=2383) - { + { cells.resize(hashsize,0); Reset(); - } + } // void Reset() - { + { for(int i=0,ni=cells.size();inext; delete pc; pc=pn; - } } + } voxelsz =0.25; puid =0; ncells =0; nprobes =1; nqueries =1; - } + } // void GarbageCollect(int lifetime=256) - { + { const int life=puid-lifetime; for(int i=0;inext; if(pc->puidnext=pn; else root=pn; delete pc;pc=pp;--ncells; - } - pp=pc;pc=pn; } + pp=pc;pc=pn; } + } //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); nqueries=1; nprobes=1; - ++puid; /* TODO: Reset puid's when int range limit is reached */ - /* else setup a priority list... */ - } + ++puid; ///@todo: Reset puid's when int range limit is reached */ + /* else setup a priority list... */ + } // int RemoveReferences(btCollisionShape* pcs) - { + { int refcount=0; for(int i=0;inext; if(pc->pclient==pcs) - { + { if(pp) pp->next=pn; else root=pn; delete pc;pc=pp;++refcount; - } - pp=pc;pc=pn; } + pp=pc;pc=pn; } - return(refcount); } + return(refcount); + } // btScalar Evaluate( const btVector3& x, - btCollisionShape* shape, - btVector3& normal, - btScalar margin) - { + btCollisionShape* shape, + btVector3& normal, + btScalar margin) + { /* Lookup cell */ const btVector3 scx=x/voxelsz; const IntFrac ix=Decompose(scx.x()); @@ -166,19 +166,19 @@ struct btSparseSdf Cell* c=root; ++nqueries; while(c) - { + { ++nprobes; if( (c->hash==h) && (c->c[0]==ix.b) && (c->c[1]==iy.b) && (c->c[2]==iz.b) && (c->pclient==shape)) - { break; } - else - { c=c->next; } - } + { break; } + else + { c=c->next; } + } if(!c) - { + { ++nprobes; ++ncells; c=new Cell(); @@ -187,82 +187,82 @@ struct btSparseSdf c->hash=h; c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; BuildCell(*c); - } + } c->puid=puid; /* Extract infos */ const int o[]={ ix.i,iy.i,iz.i}; const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+1][o[2]+1], - c->d[o[0]+0][o[1]+1][o[2]+1]}; + c->d[o[0]+1][o[1]+0][o[2]+0], + c->d[o[0]+1][o[1]+1][o[2]+0], + c->d[o[0]+0][o[1]+1][o[2]+0], + c->d[o[0]+0][o[1]+0][o[2]+1], + c->d[o[0]+1][o[1]+0][o[2]+1], + c->d[o[0]+1][o[1]+1][o[2]+1], + c->d[o[0]+0][o[1]+1][o[2]+1]}; /* Normal */ - #if 1 +#if 1 const btScalar gx[]={ d[1]-d[0],d[2]-d[3], - d[5]-d[4],d[6]-d[7]}; + d[5]-d[4],d[6]-d[7]}; const btScalar gy[]={ d[3]-d[0],d[2]-d[1], - d[7]-d[4],d[6]-d[5]}; + d[7]-d[4],d[6]-d[5]}; const btScalar gz[]={ d[4]-d[0],d[5]-d[1], - d[7]-d[3],d[6]-d[2]}; + d[7]-d[3],d[6]-d[2]}; normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), - Lerp(gx[2],gx[3],iy.f),iz.f)); + Lerp(gx[2],gx[3],iy.f),iz.f)); normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), - Lerp(gy[2],gy[3],ix.f),iz.f)); + Lerp(gy[2],gy[3],ix.f),iz.f)); normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), - Lerp(gz[2],gz[3],ix.f),iy.f)); + Lerp(gz[2],gz[3],ix.f),iy.f)); normal = normal.normalized(); - #else +#else normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); - #endif +#endif /* Distance */ const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), - Lerp(d[3],d[2],ix.f),iy.f); + Lerp(d[3],d[2],ix.f),iy.f); const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), - Lerp(d[7],d[6],ix.f),iy.f); + Lerp(d[7],d[6],ix.f),iy.f); return(Lerp(d0,d1,iz.f)-margin); - } + } // void BuildCell(Cell& c) - { + { const btVector3 org=btVector3( (btScalar)c.c[0], - (btScalar)c.c[1], - (btScalar)c.c[2]) * - CELLSIZE*voxelsz; + (btScalar)c.c[1], + (btScalar)c.c[2]) * + CELLSIZE*voxelsz; for(int k=0;k<=CELLSIZE;++k) - { + { const btScalar z=voxelsz*k+org.z(); for(int j=0;j<=CELLSIZE;++j) - { + { const btScalar y=voxelsz*j+org.y(); for(int i=0;i<=CELLSIZE;++i) - { + { const btScalar x=voxelsz*i+org.x(); c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), - c.pclient); - } + c.pclient); } } } + } // static inline btScalar DistanceToShape(const btVector3& x, - btCollisionShape* shape) - { + btCollisionShape* shape) + { btTransform unit; unit.setIdentity(); if(shape->isConvex()) - { + { btGjkEpaSolver2::sResults res; btConvexShape* csh=static_cast(shape); return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); - } - return(0); } + return(0); + } // static inline IntFrac Decompose(btScalar x) - { + { /* That one need a lot of improvements... */ /* Remove test, faster floor... */ IntFrac r; @@ -272,18 +272,18 @@ struct btSparseSdf const btScalar k=(x-r.b)*CELLSIZE; r.i=(int)k;r.f=k-r.i;r.b-=o; return(r); - } + } // static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) - { + { return(a+(b-a)*t); - } + } + - // static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape) - { + { struct btS { int x,y,z; @@ -291,16 +291,16 @@ struct btSparseSdf }; btS myset; - + myset.x=x;myset.y=y;myset.z=z;myset.p=shape; const void* ptr = &myset; unsigned int result = HsiehHash (ptr); - + return result; - } + } }; - + #endif diff --git a/extern/bullet2/src/LinearMath/CMakeLists.txt b/extern/bullet2/src/LinearMath/CMakeLists.txt index 02ffaad7228..99d5a6a2fef 100644 --- a/extern/bullet2/src/LinearMath/CMakeLists.txt +++ b/extern/bullet2/src/LinearMath/CMakeLists.txt @@ -3,7 +3,14 @@ INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src } ) -ADD_LIBRARY(LibLinearMath +SET(LinearMath_SRCS + btConvexHull.cpp + btQuickprof.cpp + btGeometryUtil.cpp + btAlignedAllocator.cpp +) + +SET(LinearMath_HDRS btAlignedObjectArray.h btList.h btPoolAllocator.h @@ -16,7 +23,6 @@ ADD_LIBRARY(LibLinearMath btScalar.h btAabbUtil2.h btConvexHull.h - btConvexHull.cpp btMinMax.h btQuaternion.h btStackAlloc.h @@ -25,11 +31,21 @@ ADD_LIBRARY(LibLinearMath btTransform.h btAlignedAllocator.h btIDebugDraw.h - btPoint3.h btQuickprof.h btTransformUtil.h - btQuickprof.cpp - btGeometryUtil.cpp - btAlignedAllocator.cpp ) +ADD_LIBRARY(LinearMath ${LinearMath_SRCS} ${LinearMath_HDRS}) +SET_TARGET_PROPERTIES(LinearMath PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(LinearMath PROPERTIES SOVERSION ${BULLET_VERSION}) + +#FILES_MATCHING requires CMake 2.6 +IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + INSTALL(TARGETS LinearMath DESTINATION lib) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") +ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + +IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(LinearMath PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(LinearMath PROPERTIES PUBLIC_HEADER "${LinearMath_HDRS}") +ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet2/src/LinearMath/btAabbUtil2.h index 275c4914628..532ce1bf633 100644 --- a/extern/bullet2/src/LinearMath/btAabbUtil2.h +++ b/extern/bullet2/src/LinearMath/btAabbUtil2.h @@ -21,6 +21,8 @@ subject to the following restrictions: #include "btVector3.h" #include "btMinMax.h" + + SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin, btVector3& aabbMax, const btVector3& expansionMin, @@ -30,15 +32,26 @@ SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin, aabbMax = aabbMax + expansionMax; } +/// conservative test for overlap between two aabbs +SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, + const btVector3 &point) +{ + bool overlap = true; + overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap; + overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap; + overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap; + return overlap; +} + /// conservative test for overlap between two aabbs SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, const btVector3 &aabbMin2, const btVector3 &aabbMax2) { bool overlap = true; - overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap; + overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap; + overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap; return overlap; } @@ -73,6 +86,7 @@ SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent) } + SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, const btVector3& rayInvDirection, const unsigned int raySign[3], @@ -82,10 +96,10 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, btScalar lambda_max) { btScalar tmax, tymin, tymax, tzmin, tzmax; - tmin = (bounds[raySign[0]][0] - rayFrom[0]) * rayInvDirection[0]; - tmax = (bounds[1-raySign[0]][0] - rayFrom[0]) * rayInvDirection[0]; - tymin = (bounds[raySign[1]][1] - rayFrom[1]) * rayInvDirection[1]; - tymax = (bounds[1-raySign[1]][1] - rayFrom[1]) * rayInvDirection[1]; + tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX(); + tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX(); + tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY(); + tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY(); if ( (tmin > tymax) || (tymin > tmax) ) return false; @@ -96,8 +110,8 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, if (tymax < tmax) tmax = tymax; - tzmin = (bounds[raySign[2]][2] - rayFrom[2]) * rayInvDirection[2]; - tzmax = (bounds[1-raySign[2]][2] - rayFrom[2]) * rayInvDirection[2]; + tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); + tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); if ( (tmin > tzmax) || (tzmin > tmax) ) return false; @@ -196,6 +210,26 @@ SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVec aabbMaxOut = center+extent; } +#define USE_BANCHLESS 1 +#ifdef USE_BANCHLESS + //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) + SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) + { + return static_cast(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) + & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) + & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), + 1, 0)); + } +#else + SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) + { + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; + } +#endif //USE_BANCHLESS #endif diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp index e120289e061..a3d790f8abc 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp @@ -19,6 +19,21 @@ int gNumAlignedAllocs = 0; int gNumAlignedFree = 0; int gTotalBytesAlignedAllocs = 0;//detect memory leaks +static void *btAllocDefault(size_t size) +{ + return malloc(size); +} + +static void btFreeDefault(void *ptr) +{ + free(ptr); +} + +static btAllocFunc *sAllocFunc = btAllocDefault; +static btFreeFunc *sFreeFunc = btFreeDefault; + + + #if defined (BT_HAS_ALIGNED_ALLOCATOR) #include static void *btAlignedAllocDefault(size_t size, int alignment) @@ -49,7 +64,7 @@ static inline void *btAlignedAllocDefault(size_t size, int alignment) char *real; unsigned long offset; - real = (char *)malloc(size + sizeof(void *) + (alignment-1)); + real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1)); if (real) { offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1); ret = (void *)((real + sizeof(void *)) + offset); @@ -66,25 +81,14 @@ static inline void btAlignedFreeDefault(void *ptr) if (ptr) { real = *((void **)(ptr)-1); - free(real); + sFreeFunc(real); } } #endif -static void *btAllocDefault(size_t size) -{ - return malloc(size); -} - -static void btFreeDefault(void *ptr) -{ - free(ptr); -} static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault; static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault; -static btAllocFunc *sAllocFunc = btAllocDefault; -static btFreeFunc *sFreeFunc = btFreeDefault; void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc) { diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.h b/extern/bullet2/src/LinearMath/btAlignedAllocator.h index a252f324d77..f168f3c66c7 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.h +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.h @@ -38,7 +38,7 @@ void btAlignedFreeInternal (void* ptr,int line,char* filename); void* btAlignedAllocInternal (size_t size, int alignment); void btAlignedFreeInternal (void* ptr); - #define btAlignedAlloc(a,b) btAlignedAllocInternal(a,b) + #define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment) #define btAlignedFree(ptr) btAlignedFreeInternal(ptr) #endif @@ -49,8 +49,11 @@ typedef void (btAlignedFreeFunc)(void *memblock); typedef void *(btAllocFunc)(size_t size); typedef void (btFreeFunc)(void *memblock); -void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc); +///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc); +///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it. +void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc); + ///The btAlignedAllocator is a portable class for aligned memory allocations. ///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned. diff --git a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h index 5598f0d7236..bad1eee1f63 100644 --- a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h +++ b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h @@ -58,7 +58,7 @@ class btAlignedObjectArray { return (size ? size*2 : 1); } - SIMD_FORCE_INLINE void copy(int start,int end, T* dest) + SIMD_FORCE_INLINE void copy(int start,int end, T* dest) const { int i; for (i=start;iid = m_tris.size(); m_tris.push_back(tr); return tr; } -void HullLibrary::deAllocateTriangle(Tri* tri) +void HullLibrary::deAllocateTriangle(btHullTriangle* tri) { btAssert(m_tris[tri->id]==tri); m_tris[tri->id]=NULL; - tri->~Tri(); + tri->~btHullTriangle(); btAlignedFree(tri); } -void HullLibrary::extrude(Tri *t0,int v) +void HullLibrary::extrude(btHullTriangle *t0,int v) { int3 t= *t0; int n = m_tris.size(); - Tri* ta = allocateTriangle(v,t[1],t[2]); + btHullTriangle* ta = allocateTriangle(v,t[1],t[2]); ta->n = int3(t0->n[0],n+1,n+2); m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; - Tri* tb = allocateTriangle(v,t[2],t[0]); + btHullTriangle* tb = allocateTriangle(v,t[2],t[0]); tb->n = int3(t0->n[1],n+2,n+0); m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; - Tri* tc = allocateTriangle(v,t[0],t[1]); + btHullTriangle* tc = allocateTriangle(v,t[0],t[1]); tc->n = int3(t0->n[2],n+0,n+1); m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; checkit(ta); @@ -469,10 +469,10 @@ void HullLibrary::extrude(Tri *t0,int v) } -Tri* HullLibrary::extrudable(btScalar epsilon) +btHullTriangle* HullLibrary::extrudable(btScalar epsilon) { int i; - Tri *t=NULL; + btHullTriangle *t=NULL; for(i=0;iriserise)) @@ -550,23 +550,23 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point - Tri *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1); - Tri *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); - Tri *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); - Tri *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); + btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1); + btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); + btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); + btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; checkit(t0);checkit(t1);checkit(t2);checkit(t3); for(j=0;jvmax<0); btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); t->vmax = maxdirsterid(verts,verts_count,n,allow); t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); } - Tri *te; + btHullTriangle *te; vlimit-=4; while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) { @@ -594,7 +594,7 @@ int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) int3 nt=*m_tris[j]; if(above(verts,nt,center,btScalar(0.01)*epsilon) || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) { - Tri *nb = m_tris[m_tris[j]->n[0]]; + btHullTriangle *nb = m_tris[m_tris[j]->n[0]]; btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->idvmax>=0) break; btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); diff --git a/extern/bullet2/src/LinearMath/btConvexHull.h b/extern/bullet2/src/LinearMath/btConvexHull.h index 8c36307dfd6..92560bddb9c 100644 --- a/extern/bullet2/src/LinearMath/btConvexHull.h +++ b/extern/bullet2/src/LinearMath/btConvexHull.h @@ -137,13 +137,9 @@ class ConvexH }; ConvexH() { - int i; - i=0; } ~ConvexH() { - int i; - i=0; } btAlignedObjectArray vertices; btAlignedObjectArray edges; @@ -188,7 +184,7 @@ public: class HullLibrary { - btAlignedObjectArray m_tris; + btAlignedObjectArray m_tris; public: @@ -203,15 +199,15 @@ private: bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit); - class Tri* allocateTriangle(int a,int b,int c); - void deAllocateTriangle(Tri*); - void b2bfix(Tri* s,Tri*t); + class btHullTriangle* allocateTriangle(int a,int b,int c); + void deAllocateTriangle(btHullTriangle*); + void b2bfix(btHullTriangle* s,btHullTriangle*t); - void removeb2b(Tri* s,Tri*t); + void removeb2b(btHullTriangle* s,btHullTriangle*t); - void checkit(Tri *t); + void checkit(btHullTriangle *t); - Tri* extrudable(btScalar epsilon); + btHullTriangle* extrudable(btScalar epsilon); int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit); @@ -221,7 +217,7 @@ private: class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); - void extrude(class Tri* t0,int v); + void extrude(class btHullTriangle* t0,int v); ConvexH* test_cube(); diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h index 563615a9a32..e5a0061b779 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -29,6 +29,7 @@ DEALINGS IN THE SOFTWARE. #define IDEBUG_DRAW__H #include "btVector3.h" +#include "btTransform.h" ///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations. @@ -52,11 +53,26 @@ class btIDebugDraw DBG_EnableSatComparison = 256, DBG_DisableBulletLCP = 512, DBG_EnableCCD = 1024, + DBG_DrawConstraints = (1 << 11), + DBG_DrawConstraintLimits = (1 << 12), DBG_MAX_DEBUG_DRAW_MODE }; virtual ~btIDebugDraw() {}; + virtual void drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor) + { + drawLine (from, to, fromColor); + } + + virtual void drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha) + { + } + + virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color) + { + } + virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0; virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha) @@ -109,6 +125,171 @@ class btIDebugDraw edgecoord[i]*=-1.f; } } + void drawTransform(const btTransform& transform, btScalar orthoLen) + { + btVector3 start = transform.getOrigin(); + drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0)); + drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0)); + drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f)); + } + + void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, + const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f)) + { + const btVector3& vx = axis; + btVector3 vy = normal.cross(axis); + btScalar step = stepDegrees * SIMD_RADS_PER_DEG; + int nSteps = (int)((maxAngle - minAngle) / step); + if(!nSteps) nSteps = 1; + btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle); + if(drawSect) + { + drawLine(center, prev, color); + } + for(int i = 1; i <= nSteps; i++) + { + btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps); + btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle); + drawLine(prev, next, color); + prev = next; + } + if(drawSect) + { + drawLine(center, prev, color); + } + } + void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, + btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f)) + { + btVector3 vA[74]; + btVector3 vB[74]; + btVector3 *pvA = vA, *pvB = vB, *pT; + btVector3 npole = center + up * radius; + btVector3 spole = center - up * radius; + btVector3 arcStart; + btScalar step = stepDegrees * SIMD_RADS_PER_DEG; + const btVector3& kv = up; + const btVector3& iv = axis; + btVector3 jv = kv.cross(iv); + bool drawN = false; + bool drawS = false; + if(minTh <= -SIMD_HALF_PI) + { + minTh = -SIMD_HALF_PI + step; + drawN = true; + } + if(maxTh >= SIMD_HALF_PI) + { + maxTh = SIMD_HALF_PI - step; + drawS = true; + } + if(minTh > maxTh) + { + minTh = -SIMD_HALF_PI + step; + maxTh = SIMD_HALF_PI - step; + drawN = drawS = true; + } + int n_hor = (int)((maxTh - minTh) / step) + 1; + if(n_hor < 2) n_hor = 2; + btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1); + bool isClosed = false; + if(minPs > maxPs) + { + minPs = -SIMD_PI + step; + maxPs = SIMD_PI; + isClosed = true; + } + else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f)) + { + isClosed = true; + } + else + { + isClosed = false; + } + int n_vert = (int)((maxPs - minPs) / step) + 1; + if(n_vert < 2) n_vert = 2; + btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1); + for(int i = 0; i < n_hor; i++) + { + btScalar th = minTh + btScalar(i) * step_h; + btScalar sth = radius * btSin(th); + btScalar cth = radius * btCos(th); + for(int j = 0; j < n_vert; j++) + { + btScalar psi = minPs + btScalar(j) * step_v; + btScalar sps = btSin(psi); + btScalar cps = btCos(psi); + pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv; + if(i) + { + drawLine(pvA[j], pvB[j], color); + } + else if(drawS) + { + drawLine(spole, pvB[j], color); + } + if(j) + { + drawLine(pvB[j-1], pvB[j], color); + } + else + { + arcStart = pvB[j]; + } + if((i == (n_hor - 1)) && drawN) + { + drawLine(npole, pvB[j], color); + } + if(isClosed) + { + if(j == (n_vert-1)) + { + drawLine(arcStart, pvB[j], color); + } + } + else + { + if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1)))) + { + drawLine(center, pvB[j], color); + } + } + } + pT = pvA; pvA = pvB; pvB = pT; + } + } + + void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color) + { + drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color); + drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color); + drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color); + drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color); + drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color); + drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color); + drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color); + drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color); + drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color); + drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color); + drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color); + drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color); + } + void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color) + { + drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color); + drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color); + drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color); + drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color); + } }; diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h index 14aa4ae2348..e45afc3c055 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -23,14 +23,16 @@ subject to the following restrictions: -///The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3. -///Make sure to only include a pure orthogonal matrix without scaling. +/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3. + * Make sure to only include a pure orthogonal matrix without scaling. */ class btMatrix3x3 { public: + /** @brief No initializaion constructor */ btMatrix3x3 () {} // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } + /**@brief Constructor from Quaternion */ explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } /* template @@ -39,6 +41,7 @@ class btMatrix3x3 { setEulerYPR(yaw, pitch, roll); } */ + /** @brief Constructor with row major formatting */ btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& zx, const btScalar& zy, const btScalar& zz) @@ -47,14 +50,14 @@ class btMatrix3x3 { yx, yy, yz, zx, zy, zz); } - + /** @brief Copy constructor */ SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) { m_el[0] = other.m_el[0]; m_el[1] = other.m_el[1]; m_el[2] = other.m_el[2]; } - + /** @brief Assignment Operator */ SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) { m_el[0] = other.m_el[0]; @@ -63,34 +66,45 @@ class btMatrix3x3 { return *this; } + /** @brief Get a column of the matrix as a vector + * @param i Column number 0 indexed */ SIMD_FORCE_INLINE btVector3 getColumn(int i) const { return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); } - + /** @brief Get a row of the matrix as a vector + * @param i Row number 0 indexed */ SIMD_FORCE_INLINE const btVector3& getRow(int i) const { + btFullAssert(0 <= i && i < 3); return m_el[i]; } - + /** @brief Get a mutable reference to a row of the matrix as a vector + * @param i Row number 0 indexed */ SIMD_FORCE_INLINE btVector3& operator[](int i) { btFullAssert(0 <= i && i < 3); return m_el[i]; } + /** @brief Get a const reference to a row of the matrix as a vector + * @param i Row number 0 indexed */ SIMD_FORCE_INLINE const btVector3& operator[](int i) const { btFullAssert(0 <= i && i < 3); return m_el[i]; } + /** @brief Multiply by the target matrix on the right + * @param m Rotation matrix to be applied + * Equivilant to this = this * m */ btMatrix3x3& operator*=(const btMatrix3x3& m); - + /** @brief Set from a carray of btScalars + * @param m A pointer to the beginning of an array of 9 btScalars */ void setFromOpenGLSubMatrix(const btScalar *m) { m_el[0].setValue(m[0],m[4],m[8]); @@ -98,7 +112,16 @@ class btMatrix3x3 { m_el[2].setValue(m[2],m[6],m[10]); } - + /** @brief Set the values of the matrix explicitly (row major) + * @param xx Top left + * @param xy Top Middle + * @param xz Top Right + * @param yx Middle Left + * @param yy Middle Middle + * @param yz Middle Right + * @param zx Bottom Left + * @param zy Bottom Middle + * @param zz Bottom Right*/ void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& zx, const btScalar& zy, const btScalar& zz) @@ -107,7 +130,9 @@ class btMatrix3x3 { m_el[1].setValue(yx,yy,yz); m_el[2].setValue(zx,zy,zz); } - + + /** @brief Set the matrix from a quaternion + * @param q The Quaternion to match */ void setRotation(const btQuaternion& q) { btScalar d = q.length2(); @@ -123,35 +148,27 @@ class btMatrix3x3 { } - + /** @brief Set the matrix from euler angles using YPR around YXZ respectively + * @param yaw Yaw about Y axis + * @param pitch Pitch about X axis + * @param roll Roll about Z axis + */ void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { - - btScalar cy(btCos(yaw)); - btScalar sy(btSin(yaw)); - btScalar cp(btCos(pitch)); - btScalar sp(btSin(pitch)); - btScalar cr(btCos(roll)); - btScalar sr(btSin(roll)); - btScalar cc = cy * cr; - btScalar cs = cy * sr; - btScalar sc = sy * cr; - btScalar ss = sy * sr; - setValue(cc - sp * ss, -cs - sp * sc, -sy * cp, - cp * sr, cp * cr, -sp, - sc + sp * cs, -ss + sp * cc, cy * cp); - + setEulerZYX(roll, pitch, yaw); } - /** - * setEulerZYX - * @param euler a const reference to a btVector3 of euler angles + /** @brief Set the matrix from euler angles YPR around ZYX axes + * @param eulerX Roll about X axis + * @param eulerY Pitch around Y axis + * @param eulerZ Yaw aboud Z axis + * * These angles are used to produce a rotation matrix. The euler * angles are applied in ZYX order. I.e a vector is first rotated * about X then Y and then Z **/ - - void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { + void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { + ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code btScalar ci ( btCos(eulerX)); btScalar cj ( btCos(eulerY)); btScalar ch ( btCos(eulerZ)); @@ -168,13 +185,24 @@ class btMatrix3x3 { -sj, cj * si, cj * ci); } + /**@brief Set the matrix to the identity */ void setIdentity() { setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), btScalar(0.0), btScalar(1.0), btScalar(0.0), btScalar(0.0), btScalar(0.0), btScalar(1.0)); } - + + static const btMatrix3x3& getIdentity() + { + static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), + btScalar(0.0), btScalar(1.0), btScalar(0.0), + btScalar(0.0), btScalar(0.0), btScalar(1.0)); + return identityMatrix; + } + + /**@brief Fill the values of the matrix into a 9 element array + * @param m The array to be filled */ void getOpenGLSubMatrix(btScalar *m) const { m[0] = btScalar(m_el[0].x()); @@ -191,6 +219,8 @@ class btMatrix3x3 { m[11] = btScalar(0.0); } + /**@brief Get the matrix represented as a quaternion + * @param q The quaternion which will be set */ void getRotation(btQuaternion& q) const { btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); @@ -224,35 +254,102 @@ class btMatrix3x3 { } q.setValue(temp[0],temp[1],temp[2],temp[3]); } - - void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const + + /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR + * @param yaw Yaw around Y axis + * @param pitch Pitch around X axis + * @param roll around Z axis */ + void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const { - if (btScalar(m_el[1].z()) < btScalar(1)) - { - if (btScalar(m_el[1].z()) > -btScalar(1)) - { - yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); - pitch = btScalar(btAsin(-m_el[1].y())); - roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); - } - else - { - yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z())); - pitch = SIMD_HALF_PI; - roll = btScalar(0.0); - } - } - else + // first use the normal calculus + yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); + pitch = btScalar(btAsin(-m_el[2].x())); + roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); + + // on pitch = +/-HalfPI + if (btFabs(pitch)==SIMD_HALF_PI) { - yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z())); - pitch = -SIMD_HALF_PI; - roll = btScalar(0.0); + if (yaw>0) + yaw-=SIMD_PI; + else + yaw+=SIMD_PI; + + if (roll>0) + roll-=SIMD_PI; + else + roll+=SIMD_PI; } - } - - + }; + + + /**@brief Get the matrix represented as euler angles around ZYX + * @param yaw Yaw around X axis + * @param pitch Pitch around Y axis + * @param roll around X axis + * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ + void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const + { + struct Euler{btScalar yaw, pitch, roll;}; + Euler euler_out; + Euler euler_out2; //second solution + //get the pointer to the raw data + + // Check that pitch is not at a singularity + if (btFabs(m_el[2].x()) >= 1) + { + euler_out.yaw = 0; + euler_out2.yaw = 0; + // From difference of angles formula + btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); + if (m_el[2].x() > 0) //gimbal locked up + { + euler_out.pitch = SIMD_PI / btScalar(2.0); + euler_out2.pitch = SIMD_PI / btScalar(2.0); + euler_out.roll = euler_out.pitch + delta; + euler_out2.roll = euler_out.pitch + delta; + } + else // gimbal locked down + { + euler_out.pitch = -SIMD_PI / btScalar(2.0); + euler_out2.pitch = -SIMD_PI / btScalar(2.0); + euler_out.roll = -euler_out.pitch + delta; + euler_out2.roll = -euler_out.pitch + delta; + } + } + else + { + euler_out.pitch = - btAsin(m_el[2].x()); + euler_out2.pitch = SIMD_PI - euler_out.pitch; + + euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), + m_el[2].z()/btCos(euler_out.pitch)); + euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), + m_el[2].z()/btCos(euler_out2.pitch)); + + euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), + m_el[0].x()/btCos(euler_out.pitch)); + euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), + m_el[0].x()/btCos(euler_out2.pitch)); + } + + if (solution_number == 1) + { + yaw = euler_out.yaw; + pitch = euler_out.pitch; + roll = euler_out.roll; + } + else + { + yaw = euler_out2.yaw; + pitch = euler_out2.pitch; + roll = euler_out2.roll; + } + } + + /**@brief Create a scaled copy of the matrix + * @param s Scaling vector The elements of the vector will scale each column */ btMatrix3x3 scaled(const btVector3& s) const { @@ -261,10 +358,15 @@ class btMatrix3x3 { m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); } + /**@brief Return the determinant of the matrix */ btScalar determinant() const; + /**@brief Return the adjoint of the matrix */ btMatrix3x3 adjoint() const; + /**@brief Return the matrix with all values non negative */ btMatrix3x3 absolute() const; + /**@brief Return the transpose of the matrix */ btMatrix3x3 transpose() const; + /**@brief Return the inverse of the matrix */ btMatrix3x3 inverse() const; btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; @@ -284,12 +386,15 @@ class btMatrix3x3 { } - ///diagonalizes this matrix by the Jacobi method. rot stores the rotation - ///from the coordinate system in which the matrix is diagonal to the original - ///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration - ///stops when all off-diagonal elements are less than the threshold multiplied - ///by the sum of the absolute values of the diagonal, or when maxSteps have - ///been executed. Note that this matrix is assumed to be symmetric. + /**@brief diagonalizes this matrix by the Jacobi method. + * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original + * coordinate system, i.e., old_this = rot * new_this * rot^T. + * @param threshold See iteration + * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied + * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. + * + * Note that this matrix is assumed to be symmetric. + */ void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) { rot.setIdentity(); @@ -371,11 +476,18 @@ class btMatrix3x3 { protected: + /**@brief Calculate the matrix cofactor + * @param r1 The first row to use for calculating the cofactor + * @param c1 The first column to use for calculating the cofactor + * @param r1 The second row to use for calculating the cofactor + * @param c1 The second column to use for calculating the cofactor + * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details + */ btScalar cofac(int r1, int c1, int r2, int c2) const { return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; } - + ///Data storage for the matrix, each vector is a row of the matrix btVector3 m_el[3]; }; @@ -494,6 +606,8 @@ class btMatrix3x3 { } */ +/**@brief Equality operator between two matrices + * It will test all elements are equal. */ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) { return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet2/src/LinearMath/btQuadWord.h index 2e80fc2ca47..c657afd2bb1 100644 --- a/extern/bullet2/src/LinearMath/btQuadWord.h +++ b/extern/bullet2/src/LinearMath/btQuadWord.h @@ -18,121 +18,159 @@ subject to the following restrictions: #include "btScalar.h" #include "btMinMax.h" -#include +#if defined (__CELLOS_LV2) && defined (__SPU__) +#include +#endif -///The btQuadWordStorage class is base class for btVector3 and btQuaternion. -///Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. todo: look into this -///ATTRIBUTE_ALIGNED16(class) btQuadWordStorage -class btQuadWordStorage +/**@brief The btQuadWord class is base class for btVector3 and btQuaternion. + * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. + */ +#ifndef USE_LIBSPE2 +ATTRIBUTE_ALIGNED16(class) btQuadWord +#else +class btQuadWord +#endif { protected: - btScalar m_x; - btScalar m_y; - btScalar m_z; - btScalar m_unusedW; - +#if defined (__SPU__) && defined (__CELLOS_LV2__) + union { + vec_float4 mVec128; + btScalar m_floats[4]; + }; public: + vec_float4 get128() const + { + return mVec128; + } +protected: +#else //__CELLOS_LV2__ __SPU__ + btScalar m_floats[4]; +#endif //__CELLOS_LV2__ __SPU__ -}; - - -///btQuadWord is base-class for vectors, points -class btQuadWord : public btQuadWordStorage -{ public: - -// SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } -// SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; } - - SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; } - - SIMD_FORCE_INLINE const btScalar& getY() const { return m_y; } - - SIMD_FORCE_INLINE const btScalar& getZ() const { return m_z; } - - SIMD_FORCE_INLINE void setX(btScalar x) { m_x = x;}; - - SIMD_FORCE_INLINE void setY(btScalar y) { m_y = y;}; - - SIMD_FORCE_INLINE void setZ(btScalar z) { m_z = z;}; - - SIMD_FORCE_INLINE void setW(btScalar w) { m_unusedW = w;}; - - SIMD_FORCE_INLINE const btScalar& x() const { return m_x; } - - SIMD_FORCE_INLINE const btScalar& y() const { return m_y; } - - SIMD_FORCE_INLINE const btScalar& z() const { return m_z; } - - SIMD_FORCE_INLINE const btScalar& w() const { return m_unusedW; } - - - SIMD_FORCE_INLINE operator btScalar *() { return &m_x; } - SIMD_FORCE_INLINE operator const btScalar *() const { return &m_x; } - - - + + + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;}; + /**@brief Set the y value */ + SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;}; + /**@brief Set the z value */ + SIMD_FORCE_INLINE void setZ(btScalar z) { m_floats[2] = z;}; + /**@brief Set the w value */ + SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;}; + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } + /**@brief Return the w value */ + SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } + + //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } + //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } + ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. + SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } + SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } + + SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const + { + return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); + } + + SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const + { + return !(*this == other); + } + + /**@brief Set x,y,z and zero w + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) { - m_x=x; - m_y=y; - m_z=z; - m_unusedW = 0.f; + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3] = 0.f; } /* void getValue(btScalar *m) const { - m[0] = m_x; - m[1] = m_y; - m[2] = m_z; + m[0] = m_floats[0]; + m[1] = m_floats[1]; + m[2] = m_floats[2]; } */ +/**@brief Set the values + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) { - m_x=x; - m_y=y; - m_z=z; - m_unusedW=w; + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3]=w; } - + /**@brief No initialization constructor */ SIMD_FORCE_INLINE btQuadWord() - // :m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.)) - { - } - - SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q) + // :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.)) { - *((btQuadWordStorage*)this) = q; } - + + /**@brief Three argument constructor (zeros w) + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z) { - m_x = x, m_y = y, m_z = z, m_unusedW = 0.0f; + m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; } +/**@brief Initializing constructor + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) { - m_x = x, m_y = y, m_z = z, m_unusedW = w; + m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; } - + /**@brief Set each element to the max of the current values and the values of another btQuadWord + * @param other The other btQuadWord to compare with + */ SIMD_FORCE_INLINE void setMax(const btQuadWord& other) { - btSetMax(m_x, other.m_x); - btSetMax(m_y, other.m_y); - btSetMax(m_z, other.m_z); - btSetMax(m_unusedW, other.m_unusedW); + btSetMax(m_floats[0], other.m_floats[0]); + btSetMax(m_floats[1], other.m_floats[1]); + btSetMax(m_floats[2], other.m_floats[2]); + btSetMax(m_floats[3], other.m_floats[3]); } - + /**@brief Set each element to the min of the current values and the values of another btQuadWord + * @param other The other btQuadWord to compare with + */ SIMD_FORCE_INLINE void setMin(const btQuadWord& other) { - btSetMin(m_x, other.m_x); - btSetMin(m_y, other.m_y); - btSetMin(m_z, other.m_z); - btSetMin(m_unusedW, other.m_unusedW); + btSetMin(m_floats[0], other.m_floats[0]); + btSetMin(m_floats[1], other.m_floats[1]); + btSetMin(m_floats[2], other.m_floats[2]); + btSetMin(m_floats[3], other.m_floats[3]); } diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index 264751b33e7..cbeca2681cc 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -17,39 +17,56 @@ subject to the following restrictions: #ifndef SIMD__QUATERNION_H_ #define SIMD__QUATERNION_H_ + #include "btVector3.h" +#include "btQuadWord.h" -///The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. +/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */ class btQuaternion : public btQuadWord { public: + /**@brief No initialization constructor */ btQuaternion() {} // template // explicit Quaternion(const btScalar *v) : Tuple4(v) {} - + /**@brief Constructor from scalars */ btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) : btQuadWord(x, y, z, w) {} - + /**@brief Axis angle Constructor + * @param axis The axis which the rotation is around + * @param angle The magnitude of the rotation around the angle (Radians) */ btQuaternion(const btVector3& axis, const btScalar& angle) { setRotation(axis, angle); } - + /**@brief Constructor from Euler angles + * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z + * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y + * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */ btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { +#ifndef BT_EULER_DEFAULT_ZYX setEuler(yaw, pitch, roll); +#else + setEulerZYX(yaw, pitch, roll); +#endif } - + /**@brief Set the rotation using axis angle notation + * @param axis The axis around which to rotate + * @param angle The magnitude of the rotation in Radians */ void setRotation(const btVector3& axis, const btScalar& angle) { btScalar d = axis.length(); - assert(d != btScalar(0.0)); + btAssert(d != btScalar(0.0)); btScalar s = btSin(angle * btScalar(0.5)) / d; setValue(axis.x() * s, axis.y() * s, axis.z() * s, btCos(angle * btScalar(0.5))); } - + /**@brief Set the quaternion using Euler angles + * @param yaw Angle around Y + * @param pitch Angle around X + * @param roll Angle around Z */ void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { btScalar halfYaw = btScalar(yaw) * btScalar(0.5); @@ -66,122 +83,165 @@ public: sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); } - - btQuaternion& operator+=(const btQuaternion& q) + /**@brief Set the quaternion using euler angles + * @param yaw Angle around Z + * @param pitch Angle around Y + * @param roll Angle around X */ + void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) { - m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW; + btScalar halfYaw = btScalar(yaw) * btScalar(0.5); + btScalar halfPitch = btScalar(pitch) * btScalar(0.5); + btScalar halfRoll = btScalar(roll) * btScalar(0.5); + btScalar cosYaw = btCos(halfYaw); + btScalar sinYaw = btSin(halfYaw); + btScalar cosPitch = btCos(halfPitch); + btScalar sinPitch = btSin(halfPitch); + btScalar cosRoll = btCos(halfRoll); + btScalar sinRoll = btSin(halfRoll); + setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x + cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y + cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z + cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx + } + /**@brief Add two quaternions + * @param q The quaternion to add to this one */ + SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) + { + m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3]; return *this; } + /**@brief Subtract out a quaternion + * @param q The quaternion to subtract from this one */ btQuaternion& operator-=(const btQuaternion& q) { - m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW; + m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3]; return *this; } + /**@brief Scale this quaternion + * @param s The scalar to scale by */ btQuaternion& operator*=(const btScalar& s) { - m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s; + m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s; return *this; } - + /**@brief Multiply this quaternion by q on the right + * @param q The other quaternion + * Equivilant to this = this * q */ btQuaternion& operator*=(const btQuaternion& q) { - setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(), - m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(), - m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(), - m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z()); + setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), + m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), + m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), + m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); return *this; } - + /**@brief Return the dot product between this quaternion and another + * @param q The other quaternion */ btScalar dot(const btQuaternion& q) const { - return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW; + return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3]; } + /**@brief Return the length squared of the quaternion */ btScalar length2() const { return dot(*this); } + /**@brief Return the length of the quaternion */ btScalar length() const { return btSqrt(length2()); } + /**@brief Normalize the quaternion + * Such that x^2 + y^2 + z^2 +w^2 = 1 */ btQuaternion& normalize() { return *this /= length(); } + /**@brief Return a scaled version of this quaternion + * @param s The scale factor */ SIMD_FORCE_INLINE btQuaternion operator*(const btScalar& s) const { - return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s); + return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s); } - + /**@brief Return an inversely scaled versionof this quaternion + * @param s The inverse scale factor */ btQuaternion operator/(const btScalar& s) const { - assert(s != btScalar(0.0)); + btAssert(s != btScalar(0.0)); return *this * (btScalar(1.0) / s); } - + /**@brief Inversely scale this quaternion + * @param s The scale factor */ btQuaternion& operator/=(const btScalar& s) { - assert(s != btScalar(0.0)); + btAssert(s != btScalar(0.0)); return *this *= btScalar(1.0) / s; } - + /**@brief Return a normalized version of this quaternion */ btQuaternion normalized() const { return *this / length(); } - + /**@brief Return the angle between this quaternion and the other + * @param q The other quaternion */ btScalar angle(const btQuaternion& q) const { btScalar s = btSqrt(length2() * q.length2()); - assert(s != btScalar(0.0)); + btAssert(s != btScalar(0.0)); return btAcos(dot(q) / s); } - + /**@brief Return the angle of rotation represented by this quaternion */ btScalar getAngle() const { - btScalar s = btScalar(2.) * btAcos(m_unusedW); + btScalar s = btScalar(2.) * btAcos(m_floats[3]); return s; } - + /**@brief Return the inverse of this quaternion */ btQuaternion inverse() const { - return btQuaternion(-m_x, -m_y, -m_z, m_unusedW); + return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); } + /**@brief Return the sum of this quaternion and the other + * @param q2 The other quaternion */ SIMD_FORCE_INLINE btQuaternion operator+(const btQuaternion& q2) const { const btQuaternion& q1 = *this; - return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW); + return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); } + /**@brief Return the difference between this quaternion and the other + * @param q2 The other quaternion */ SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q2) const { const btQuaternion& q1 = *this; - return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW); + return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); } + /**@brief Return the negative of this quaternion + * This simply negates each element */ SIMD_FORCE_INLINE btQuaternion operator-() const { const btQuaternion& q2 = *this; - return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_unusedW); + return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); } - + /**@todo document this and it's use */ SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const { btQuaternion diff,sum; @@ -192,6 +252,10 @@ public: return (-qd); } + /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion + * @param q The other quaternion to interpolate with + * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. + * Slerp interpolates assuming constant velocity. */ btQuaternion slerp(const btQuaternion& q, const btScalar& t) const { btScalar theta = angle(q); @@ -200,10 +264,10 @@ public: btScalar d = btScalar(1.0) / btSin(theta); btScalar s0 = btSin((btScalar(1.0) - t) * theta); btScalar s1 = btSin(t * theta); - return btQuaternion((m_x * s0 + q.x() * s1) * d, - (m_y * s0 + q.y() * s1) * d, - (m_z * s0 + q.z() * s1) * d, - (m_unusedW * s0 + q.m_unusedW * s1) * d); + return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d, + (m_floats[1] * s0 + q.y() * s1) * d, + (m_floats[2] * s0 + q.z() * s1) * d, + (m_floats[3] * s0 + q.m_floats[3] * s1) * d); } else { @@ -211,13 +275,19 @@ public: } } - SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; } + static const btQuaternion& getIdentity() + { + static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.)); + return identityQuat; + } + + SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; } }; - +/**@brief Return the negative of a quaternion */ SIMD_FORCE_INLINE btQuaternion operator-(const btQuaternion& q) { @@ -226,7 +296,7 @@ operator-(const btQuaternion& q) - +/**@brief Return the product of two quaternions */ SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q1, const btQuaternion& q2) { return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), @@ -253,6 +323,7 @@ operator*(const btVector3& w, const btQuaternion& q) -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); } +/**@brief Calculate the dot product between two quaternions */ SIMD_FORCE_INLINE btScalar dot(const btQuaternion& q1, const btQuaternion& q2) { @@ -260,25 +331,32 @@ dot(const btQuaternion& q1, const btQuaternion& q2) } +/**@brief Return the length of a quaternion */ SIMD_FORCE_INLINE btScalar length(const btQuaternion& q) { return q.length(); } +/**@brief Return the angle between two quaternions*/ SIMD_FORCE_INLINE btScalar angle(const btQuaternion& q1, const btQuaternion& q2) { return q1.angle(q2); } - +/**@brief Return the inverse of a quaternion*/ SIMD_FORCE_INLINE btQuaternion inverse(const btQuaternion& q) { return q.inverse(); } +/**@brief Return the result of spherical linear interpolation betwen two quaternions + * @param q1 The first quaternion + * @param q2 The second quaternion + * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 + * Slerp assumes constant velocity between positions. */ SIMD_FORCE_INLINE btQuaternion slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) { diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp index e5b1196149b..fa45d02b3d3 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.cpp +++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp @@ -110,7 +110,7 @@ void CProfileNode::Reset( void ) { TotalCalls = 0; TotalTime = 0.0f; - gProfileClock.reset(); + if ( Child ) { Child->Reset(); @@ -251,6 +251,7 @@ void CProfileManager::Stop_Profile( void ) *=============================================================================================*/ void CProfileManager::Reset( void ) { + gProfileClock.reset(); Root.Reset(); Root.Call(); FrameCounter = 0; @@ -278,5 +279,68 @@ float CProfileManager::Get_Time_Since_Reset( void ) return (float)time / Profile_Get_Tick_Rate(); } +#include + +void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing) +{ + profileIterator->First(); + if (profileIterator->Is_Done()) + return; + + float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time(); + int i; + int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); + for (i=0;iGet_Current_Parent_Name(), parent_time ); + float totalTime = 0.f; + + + int numChildren = 0; + + for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next()) + { + numChildren++; + float current_total_time = profileIterator->Get_Current_Total_Time(); + accumulated_time += current_total_time; + float fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f; + { + int i; for (i=0;iGet_Current_Name(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls()); + totalTime += current_total_time; + //recurse into children + } + + if (parent_time < accumulated_time) + { + printf("what's wrong\n"); + } + for (i=0;i SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); + + for (i=0;iEnter_Child(i); + dumpRecursive(profileIterator,spacing+3); + profileIterator->Enter_Parent(); + } +} + + + +void CProfileManager::dumpAll() +{ + CProfileIterator* profileIterator = 0; + profileIterator = CProfileManager::Get_Iterator(); + + dumpRecursive(profileIterator,0); + + CProfileManager::Release_Iterator(profileIterator); +} + + + #endif //USE_BT_CLOCK diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h index b033940ca5c..f8d47c36861 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -10,14 +10,20 @@ // Credits: The Clock class was inspired by the Timer classes in // Ogre (www.ogre3d.org). + + #ifndef QUICK_PROF_H #define QUICK_PROF_H +//To disable built-in profiling, please comment out next line +//#define BT_NO_PROFILE 1 +#ifndef BT_NO_PROFILE + #include "btScalar.h" #include "LinearMath/btAlignedAllocator.h" #include -//To disable built-in profiling, please comment out next line -//#define BT_NO_PROFILE 1 + + //if you don't need btClock, you can comment next line @@ -321,6 +327,10 @@ public: } static void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); } + static void dumpRecursive(CProfileIterator* profileIterator, int spacing); + + static void dumpAll(); + private: static CProfileNode Root; static CProfileNode * CurrentNode; @@ -344,12 +354,14 @@ public: } }; -#if !defined(BT_NO_PROFILE) + #define BT_PROFILE( name ) CProfileSample __profile( name ) + #else + #define BT_PROFILE( name ) -#endif +#endif //#ifndef BT_NO_PROFILE diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index e8433405b20..822296164c1 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -18,13 +18,14 @@ subject to the following restrictions: #define SIMD___SCALAR_H #include + #include //size_t for MSVC 6.0 #include #include #include -#define BT_BULLET_VERSION 272 +#define BT_BULLET_VERSION 274 inline int btGetVersion() { @@ -44,7 +45,7 @@ inline int btGetVersion() #define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED128(a) a #else - #define BT_HAS_ALIGNED_ALLOCATOR + //#define BT_HAS_ALIGNED_ALLOCATOR #pragma warning(disable : 4324) // disable padding warning // #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. // #pragma warning(disable:4996) //Turn off warnings about deprecated C routines @@ -60,12 +61,18 @@ inline int btGetVersion() #define BT_HAVE_NATIVE_FSEL #define btFsel(a,b,c) __fsel((a),(b),(c)) #else + +#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) #define BT_USE_SSE - #endif + #include +#endif + + #endif//_XBOX + #endif //__MINGW32__ #include -#if defined(DEBUG) || defined (_DEBUG) +#ifdef BT_DEBUG #define btAssert assert #else #define btAssert(x) @@ -85,7 +92,11 @@ inline int btGetVersion() #ifndef assert #include #endif +#ifdef BT_DEBUG #define btAssert assert +#else + #define btAssert(x) +#endif //btFullAssert is optional, slows down a lot #define btFullAssert(x) @@ -102,7 +113,11 @@ inline int btGetVersion() #ifndef assert #include #endif +#ifdef BT_DEBUG #define btAssert assert +#else + #define btAssert(x) +#endif //btFullAssert is optional, slows down a lot #define btFullAssert(x) @@ -115,6 +130,9 @@ inline int btGetVersion() //non-windows systems #define SIMD_FORCE_INLINE inline + ///@todo: check out alignment methods for other platforms/compilers + ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED128(a) a #ifndef assert @@ -141,10 +159,6 @@ inline int btGetVersion() /// older compilers (gcc 3.x) and Sun needs double version of sqrt etc. /// exclude Apple Intel (i's assumed to be a Macbook or new Intel Dual Core Processor) #if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__)) -/* XXX Need to fix these... needed for SunOS 5.8 */ -#define sinf(a) sin((double)(a)) -#define cosf(a) cos((double)(a)) -#define fabsf(a) fabs((double)(a)) //use slow double float precision operation on those platforms #ifndef BT_USE_DOUBLE_PRECISION #define BT_FORCE_DOUBLE_FUNCTIONS @@ -198,7 +212,7 @@ SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) tempf = y; *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ x = tempf; - z = y*btScalar(0.5); /* hoist out the ô/2ö */ + z = y*btScalar(0.5); /* hoist out the “/2” */ x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ x = (btScalar(1.5)*x)-(x*x)*(x*z); x = (btScalar(1.5)*x)-(x*x)*(x*z); diff --git a/extern/bullet2/src/LinearMath/btStackAlloc.h b/extern/bullet2/src/LinearMath/btStackAlloc.h index 050d44bdfe9..397b084877f 100644 --- a/extern/bullet2/src/LinearMath/btStackAlloc.h +++ b/extern/bullet2/src/LinearMath/btStackAlloc.h @@ -23,6 +23,7 @@ Nov.2006 #include "btScalar.h" //for btAssert #include "btAlignedAllocator.h" +///The btBlock class is an internal structure for the btStackAlloc memory allocator. struct btBlock { btBlock* previous; diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h index a8cdb428100..c4fe33eecd7 100644 --- a/extern/bullet2/src/LinearMath/btTransform.h +++ b/extern/bullet2/src/LinearMath/btTransform.h @@ -21,34 +21,39 @@ subject to the following restrictions: #include "btMatrix3x3.h" -///The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear. -///It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. +/**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear. + *It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */ class btTransform { public: - + /**@brief No initialization constructor */ btTransform() {} - + /**@brief Constructor from btQuaternion (optional btVector3 ) + * @param q Rotation from quaternion + * @param c Translation from Vector (default 0,0,0) */ explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q, const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) : m_basis(q), m_origin(c) {} + /**@brief Constructor from btMatrix3x3 (optional btVector3) + * @param b Rotation from Matrix + * @param c Translation from Vector default (0,0,0)*/ explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b, const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) : m_basis(b), m_origin(c) {} - + /**@brief Copy constructor */ SIMD_FORCE_INLINE btTransform (const btTransform& other) : m_basis(other.m_basis), m_origin(other.m_origin) { } - + /**@brief Assignment Operator */ SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other) { m_basis = other.m_basis; @@ -57,6 +62,10 @@ public: } + /**@brief Set the current transform as the value of the product of two transforms + * @param t1 Transform 1 + * @param t2 Transform 2 + * This = Transform1 * Transform2 */ SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) { m_basis = t1.m_basis * t2.m_basis; m_origin = t1(t2.m_origin); @@ -69,7 +78,7 @@ public: } */ - +/**@brief Return the transform of the vector */ SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const { return btVector3(m_basis[0].dot(x) + m_origin.x(), @@ -77,17 +86,29 @@ public: m_basis[2].dot(x) + m_origin.z()); } + /**@brief Return the transform of the vector */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const { return (*this)(x); } + /**@brief Return the transform of the btQuaternion */ + SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q) const + { + return getRotation() * q; + } + + /**@brief Return the basis matrix for the rotation */ SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; } + /**@brief Return the basis matrix for the rotation */ SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; } + /**@brief Return the origin vector translation */ SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; } + /**@brief Return the origin vector translation */ SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; } + /**@brief Return a quaternion representing the rotation */ btQuaternion getRotation() const { btQuaternion q; m_basis.getRotation(q); @@ -95,12 +116,16 @@ public: } + /**@brief Set from an array + * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ void setFromOpenGLMatrix(const btScalar *m) { m_basis.setFromOpenGLSubMatrix(m); m_origin.setValue(m[12],m[13],m[14]); } + /**@brief Fill an array representation + * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ void getOpenGLMatrix(btScalar *m) const { m_basis.getOpenGLSubMatrix(m); @@ -110,6 +135,8 @@ public: m[15] = btScalar(1.0); } + /**@brief Set the translational element + * @param origin The vector to set the translation to */ SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) { m_origin = origin; @@ -118,26 +145,28 @@ public: SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const; - + /**@brief Set the rotational element by btMatrix3x3 */ SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis) { m_basis = basis; } + /**@brief Set the rotational element by btQuaternion */ SIMD_FORCE_INLINE void setRotation(const btQuaternion& q) { m_basis.setRotation(q); } - + /**@brief Set this transformation to the identity */ void setIdentity() { m_basis.setIdentity(); m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); } - + /**@brief Multiply this Transform by another(this = this * another) + * @param t The other transform */ btTransform& operator*=(const btTransform& t) { m_origin += m_basis * t.m_origin; @@ -145,26 +174,32 @@ public: return *this; } + /**@brief Return the inverse of this transform */ btTransform inverse() const { btMatrix3x3 inv = m_basis.transpose(); return btTransform(inv, inv * -m_origin); } + /**@brief Return the inverse of this transform times the other transform + * @param t The other transform + * return this.inverse() * the other */ btTransform inverseTimes(const btTransform& t) const; + /**@brief Return the product of this transform and the other */ btTransform operator*(const btTransform& t) const; - static btTransform getIdentity() + /**@brief Return an identity transform */ + static const btTransform& getIdentity() { - btTransform tr; - tr.setIdentity(); - return tr; + static const btTransform identityTransform(btMatrix3x3::getIdentity()); + return identityTransform; } private: - + ///Storage for the rotation btMatrix3x3 m_basis; + ///Storage for the translation btVector3 m_origin; }; @@ -191,6 +226,7 @@ btTransform::operator*(const btTransform& t) const (*this)(t.m_origin)); } +/**@brief Test if two transforms have all elements equal */ SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2) { return ( t1.getBasis() == t2.getBasis() && diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index 86ee1da5edf..e8328da4ca6 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -100,6 +100,38 @@ public: predictedTransform.setRotation(predictedOrn); } + static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel) + { + linVel = (pos1 - pos0) / timeStep; + btVector3 axis; + btScalar angle; + if (orn0 != orn1) + { + calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle); + angVel = axis * angle / timeStep; + } else + { + angVel.setValue(0,0,0); + } + } + + static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle) + { + btQuaternion orn1 = orn0.farthest(orn1a); + btQuaternion dorn = orn1 * orn0.inverse(); + ///floating point inaccuracy can lead to w component > 1..., which breaks + dorn.normalize(); + angle = dorn.getAngle(); + axis = btVector3(dorn.x(),dorn.y(),dorn.z()); + axis[3] = btScalar(0.); + //check for axis length + btScalar len = axis.length2(); + if (len < SIMD_EPSILON*SIMD_EPSILON) + axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); + else + axis /= btSqrt(len); + } + static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel) { linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; @@ -111,20 +143,11 @@ public: static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle) { - - #ifdef USE_QUATERNION_DIFF - btQuaternion orn0 = transform0.getRotation(); - btQuaternion orn1a = transform1.getRotation(); - btQuaternion orn1 = orn0.farthest(orn1a); - btQuaternion dorn = orn1 * orn0.inverse(); -#else btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse(); btQuaternion dorn; dmat.getRotation(dorn); -#endif//USE_QUATERNION_DIFF - - ///floating point inaccuracy can lead to w component > 1..., which breaks + ///floating point inaccuracy can lead to w component > 1..., which breaks dorn.normalize(); angle = dorn.getAngle(); @@ -140,5 +163,85 @@ public: }; + +///The btConvexSeparatingDistanceUtil can help speed up convex collision detection +///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance +class btConvexSeparatingDistanceUtil +{ + btQuaternion m_ornA; + btQuaternion m_ornB; + btVector3 m_posA; + btVector3 m_posB; + + btVector3 m_separatingNormal; + + btScalar m_boundingRadiusA; + btScalar m_boundingRadiusB; + btScalar m_separatingDistance; + +public: + + btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB) + :m_boundingRadiusA(boundingRadiusA), + m_boundingRadiusB(boundingRadiusB), + m_separatingDistance(0.f) + { + } + + btScalar getConservativeSeparatingDistance() + { + return m_separatingDistance; + } + + void updateSeparatingDistance(const btTransform& transA,const btTransform& transB) + { + const btVector3& toPosA = transA.getOrigin(); + const btVector3& toPosB = transB.getOrigin(); + btQuaternion toOrnA = transA.getRotation(); + btQuaternion toOrnB = transB.getRotation(); + + if (m_separatingDistance>0.f) + { + + + btVector3 linVelA,angVelA,linVelB,angVelB; + btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA); + btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB); + btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB; + btVector3 relLinVel = (linVelB-linVelA); + btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal); + if (relLinVelocLength<0.f) + { + relLinVelocLength = 0.f; + } + + btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength; + m_separatingDistance -= projectedMotion; + } + + m_posA = toPosA; + m_posB = toPosB; + m_ornA = toOrnA; + m_ornB = toOrnB; + } + + void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB) + { + m_separatingNormal = separatingVector; + m_separatingDistance = separatingDistance; + + const btVector3& toPosA = transA.getOrigin(); + const btVector3& toPosB = transB.getOrigin(); + btQuaternion toOrnA = transA.getRotation(); + btQuaternion toOrnB = transB.getRotation(); + m_posA = toPosA; + m_posB = toPosB; + m_ornA = toOrnA; + m_ornB = toOrnB; + } + +}; + + #endif //SIMD_TRANSFORM_UTIL_H diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 96548c6ba60..5d5c39e8587 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -17,127 +17,190 @@ subject to the following restrictions: #ifndef SIMD__VECTOR3_H #define SIMD__VECTOR3_H -#include "btQuadWord.h" -///btVector3 can be used to represent 3D points and vectors. -///It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user -///Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers -class btVector3 : public btQuadWord { +#include "btScalar.h" +#include "btScalar.h" +#include "btMinMax.h" +/**@brief btVector3 can be used to represent 3D points and vectors. + * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user + * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers + */ +ATTRIBUTE_ALIGNED16(class) btVector3 +{ public: - SIMD_FORCE_INLINE btVector3() {} - SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) - : btQuadWord(q) +#if defined (__SPU__) && defined (__CELLOS_LV2__) + union { + vec_float4 mVec128; + btScalar m_floats[4]; + }; +public: + vec_float4 get128() const { + return mVec128; } - - - SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) - :btQuadWord(x,y,z,btScalar(0.)) +public: +#else //__CELLOS_LV2__ __SPU__ +#ifdef BT_USE_SSE // WIN32 + union { + __m128 mVec128; + btScalar m_floats[4]; + }; + SIMD_FORCE_INLINE __m128 get128() const { + return mVec128; } + SIMD_FORCE_INLINE void set128(__m128 v128) + { + mVec128 = v128; + } +#else + btScalar m_floats[4]; +#endif +#endif //__CELLOS_LV2__ __SPU__ + + public: -// SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) -// : btQuadWord(x,y,z,w) -// { -// } + /**@brief No initialization constructor */ + SIMD_FORCE_INLINE btVector3() {} + + /**@brief Constructor from scalars + * @param x X value + * @param y Y value + * @param z Z value + */ + SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) + { + m_floats[0] = x; + m_floats[1] = y; + m_floats[2] = z; + m_floats[3] = btScalar(0.); + } + +/**@brief Add a vector to this one + * @param The vector to add to this one */ SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v) { - m_x += v.x(); m_y += v.y(); m_z += v.z(); + m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2]; return *this; } - + /**@brief Subtract a vector from this one + * @param The vector to subtract */ SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) { - m_x -= v.x(); m_y -= v.y(); m_z -= v.z(); + m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2]; return *this; } - + /**@brief Scale the vector + * @param s Scale factor */ SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s) { - m_x *= s; m_y *= s; m_z *= s; + m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s; return *this; } + /**@brief Inversely scale the vector + * @param s Scale factor to divide by */ SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) { btFullAssert(s != btScalar(0.0)); return *this *= btScalar(1.0) / s; } + /**@brief Return the dot product + * @param v The other vector in the dot product */ SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const { - return m_x * v.x() + m_y * v.y() + m_z * v.z(); + return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2]; } + /**@brief Return the length of the vector squared */ SIMD_FORCE_INLINE btScalar length2() const { return dot(*this); } + /**@brief Return the length of the vector */ SIMD_FORCE_INLINE btScalar length() const { return btSqrt(length2()); } + /**@brief Return the distance squared between the ends of this and another vector + * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; + /**@brief Return the distance between the ends of this and another vector + * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; + /**@brief Normalize this vector + * x^2 + y^2 + z^2 = 1 */ SIMD_FORCE_INLINE btVector3& normalize() { return *this /= length(); } + /**@brief Return a normalized version of this vector */ SIMD_FORCE_INLINE btVector3 normalized() const; + /**@brief Rotate this vector + * @param wAxis The axis to rotate about + * @param angle The angle to rotate by */ SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ); + /**@brief Return the angle between this and another vector + * @param v The other vector */ SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const { btScalar s = btSqrt(length2() * v.length2()); btFullAssert(s != btScalar(0.0)); return btAcos(dot(v) / s); } - + /**@brief Return a vector will the absolute values of each element */ SIMD_FORCE_INLINE btVector3 absolute() const { return btVector3( - btFabs(m_x), - btFabs(m_y), - btFabs(m_z)); + btFabs(m_floats[0]), + btFabs(m_floats[1]), + btFabs(m_floats[2])); } - + /**@brief Return the cross product between this and another vector + * @param v The other vector */ SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const { return btVector3( - m_y * v.z() - m_z * v.y(), - m_z * v.x() - m_x * v.z(), - m_x * v.y() - m_y * v.x()); + m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1], + m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2], + m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]); } SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const { - return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) + - m_y * (v1.z() * v2.x() - v1.x() * v2.z()) + - m_z * (v1.x() * v2.y() - v1.y() * v2.x()); + return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]); } + /**@brief Return the axis with the smallest value + * Note return values are 0,1,2 for x, y, or z */ SIMD_FORCE_INLINE int minAxis() const { - return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2); + return m_floats[0] < m_floats[1] ? (m_floats[0] return this, t=1 => return other) */ SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const { - return btVector3(m_x + (v.x() - m_x) * t, - m_y + (v.y() - m_y) * t, - m_z + (v.z() - m_z) * t); + return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, + m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, + m_floats[2] + (v.m_floats[2] -m_floats[2]) * t); } - + /**@brief Elementwise multiply this vector by the other + * @param v The other vector */ SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v) { - m_x *= v.x(); m_y *= v.y(); m_z *= v.z(); + m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2]; return *this; } - + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;}; + /**@brief Set the y value */ + SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;}; + /**@brief Set the z value */ + SIMD_FORCE_INLINE void setZ(btScalar z) {m_floats[2] = z;}; + /**@brief Set the w value */ + SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;}; + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } + /**@brief Return the w value */ + SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } + + //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } + //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } + ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. + SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } + SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } + + SIMD_FORCE_INLINE bool operator==(const btVector3& other) const + { + return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); + } + + SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const + { + return !(*this == other); + } + + /**@brief Set each element to the max of the current values and the values of another btVector3 + * @param other The other btVector3 to compare with + */ + SIMD_FORCE_INLINE void setMax(const btVector3& other) + { + btSetMax(m_floats[0], other.m_floats[0]); + btSetMax(m_floats[1], other.m_floats[1]); + btSetMax(m_floats[2], other.m_floats[2]); + btSetMax(m_floats[3], other.w()); + } + /**@brief Set each element to the min of the current values and the values of another btVector3 + * @param other The other btVector3 to compare with + */ + SIMD_FORCE_INLINE void setMin(const btVector3& other) + { + btSetMin(m_floats[0], other.m_floats[0]); + btSetMin(m_floats[1], other.m_floats[1]); + btSetMin(m_floats[2], other.m_floats[2]); + btSetMin(m_floats[3], other.w()); + } + + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3] = 0.f; + } + + void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const + { + v0->setValue(0. ,-z() ,y()); + v1->setValue(z() ,0. ,-x()); + v2->setValue(-y() ,x() ,0.); + } }; +/**@brief Return the sum of two vectors (Point symantics)*/ SIMD_FORCE_INLINE btVector3 operator+(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z()); + return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]); } +/**@brief Return the elementwise product of two vectors */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z()); + return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]); } +/**@brief Return the difference between two vectors */ SIMD_FORCE_INLINE btVector3 operator-(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z()); + return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]); } - +/**@brief Return the negative of the vector */ SIMD_FORCE_INLINE btVector3 operator-(const btVector3& v) { - return btVector3(-v.x(), -v.y(), -v.z()); + return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); } +/**@brief Return the vector scaled by s */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v, const btScalar& s) { - return btVector3(v.x() * s, v.y() * s, v.z() * s); + return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); } +/**@brief Return the vector scaled by s */ SIMD_FORCE_INLINE btVector3 operator*(const btScalar& s, const btVector3& v) { return v * s; } +/**@brief Return the vector inversely scaled by s */ SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v, const btScalar& s) { @@ -221,12 +367,14 @@ operator/(const btVector3& v, const btScalar& s) return v * (btScalar(1.0) / s); } +/**@brief Return the vector inversely scaled by s */ SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z()); + return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]); } +/**@brief Return the dot product between two vectors */ SIMD_FORCE_INLINE btScalar dot(const btVector3& v1, const btVector3& v2) { @@ -234,7 +382,7 @@ dot(const btVector3& v1, const btVector3& v2) } - +/**@brief Return the distance squared between two vectors */ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v1, const btVector3& v2) { @@ -242,18 +390,21 @@ distance2(const btVector3& v1, const btVector3& v2) } +/**@brief Return the distance between two vectors */ SIMD_FORCE_INLINE btScalar distance(const btVector3& v1, const btVector3& v2) { return v1.distance(v2); } +/**@brief Return the angle between two vectors */ SIMD_FORCE_INLINE btScalar angle(const btVector3& v1, const btVector3& v2) { return v1.angle(v2); } +/**@brief Return the cross product of two vectors */ SIMD_FORCE_INLINE btVector3 cross(const btVector3& v1, const btVector3& v2) { @@ -266,6 +417,10 @@ triple(const btVector3& v1, const btVector3& v2, const btVector3& v3) return v1.triple(v2, v3); } +/**@brief Return the linear interpolation between two vectors + * @param v1 One vector + * @param v2 The other vector + * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) { @@ -273,10 +428,6 @@ lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) } -SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) -{ - return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z(); -} SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const { @@ -316,47 +467,47 @@ public: SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) : btVector3(x,y,z) { - m_unusedW = w; + m_floats[3] = w; } SIMD_FORCE_INLINE btVector4 absolute4() const { return btVector4( - btFabs(m_x), - btFabs(m_y), - btFabs(m_z), - btFabs(m_unusedW)); + btFabs(m_floats[0]), + btFabs(m_floats[1]), + btFabs(m_floats[2]), + btFabs(m_floats[3])); } - btScalar getW() const { return m_unusedW;} + btScalar getW() const { return m_floats[3];} SIMD_FORCE_INLINE int maxAxis4() const { int maxIndex = -1; btScalar maxVal = btScalar(-1e30); - if (m_x > maxVal) + if (m_floats[0] > maxVal) { maxIndex = 0; - maxVal = m_x; + maxVal = m_floats[0]; } - if (m_y > maxVal) + if (m_floats[1] > maxVal) { maxIndex = 1; - maxVal = m_y; + maxVal = m_floats[1]; } - if (m_z > maxVal) + if (m_floats[2] > maxVal) { maxIndex = 2; - maxVal = m_z; + maxVal =m_floats[2]; } - if (m_unusedW > maxVal) + if (m_floats[3] > maxVal) { maxIndex = 3; - maxVal = m_unusedW; + maxVal = m_floats[3]; } @@ -371,25 +522,25 @@ public: { int minIndex = -1; btScalar minVal = btScalar(1e30); - if (m_x < minVal) + if (m_floats[0] < minVal) { minIndex = 0; - minVal = m_x; + minVal = m_floats[0]; } - if (m_y < minVal) + if (m_floats[1] < minVal) { minIndex = 1; - minVal = m_y; + minVal = m_floats[1]; } - if (m_z < minVal) + if (m_floats[2] < minVal) { minIndex = 2; - minVal = m_z; + minVal =m_floats[2]; } - if (m_unusedW < minVal) + if (m_floats[3] < minVal) { minIndex = 3; - minVal = m_unusedW; + minVal = m_floats[3]; } return minIndex; @@ -402,6 +553,40 @@ public: return absolute4().maxAxis4(); } + + + + /**@brief Set x,y,z and zero w + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ + + +/* void getValue(btScalar *m) const + { + m[0] = m_floats[0]; + m[1] = m_floats[1]; + m[2] =m_floats[2]; + } +*/ +/**@brief Set the values + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3]=w; + } + + + + }; -- cgit v1.2.3