[llvm-commits] [test-suite] r91782 [1/5] - in /test-suite/trunk/MultiSource/Benchmarks: ./ Bullet/ Bullet/include/ Bullet/include/BulletCollision/ Bullet/include/BulletCollision/BroadphaseCollision/ Bullet/include/BulletCollision/CollisionDispatch/ Bullet/include/BulletCollision/CollisionShapes/ Bullet/include/BulletCollision/Gimpact/ Bullet/include/BulletCollision/NarrowPhaseCollision/ Bullet/include/BulletDynamics/ Bullet/include/BulletDynamics/Character/ Bullet/include/BulletDynamics/ConstraintSolver/ Bullet/include...
Anton Korobeynikov
asl at math.spbu.ru
Sat Dec 19 12:06:02 PST 2009
Author: asl
Date: Sat Dec 19 14:05:59 2009
New Revision: 91782
URL: http://llvm.org/viewvc/llvm-project?rev=91782&view=rev
Log:
Add Bullet 2.75 physics engine.
It seems that LLVM is 20% slower that gcc 4.2 on it for me.
One can remove NO_TIME difene to obtain precise timing information & profiling.
Added:
test-suite/trunk/MultiSource/Benchmarks/Bullet/
test-suite/trunk/MultiSource/Benchmarks/Bullet/BenchmarkDemo.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/BenchmarkDemo.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/Makefile
test-suite/trunk/MultiSource/Benchmarks/Bullet/SphereTriangleDetector.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/Taru.mdl
test-suite/trunk/MultiSource/Benchmarks/Bullet/btActivatingCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btAlignedAllocator.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btAxisSweep3.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btBox2dBox2dCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btBox2dShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxBoxCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxBoxDetector.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btBroadphaseProxy.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btBvhTriangleMeshShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btCapsuleShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionDispatcher.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionObject.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionWorld.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btCompoundCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btCompoundShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConcaveShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConeShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConeTwistConstraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btContactConstraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btContactProcessing.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btContinuousConvexCollision.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btContinuousDynamicsWorld.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvex2dConvex2dAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvex2dShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexCast.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexConcaveCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexConvexAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexHull.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexHullShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexInternalShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexPlaneCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexPointCloudShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexTriangleMeshShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btCylinderShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btDbvt.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btDbvtBroadphase.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btDefaultCollisionConfiguration.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btDiscreteDynamicsWorld.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btDispatcher.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btEmptyCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btEmptyShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactBvh.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactQuantizedBvh.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeneric6DofConstraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeneric6DofSpringConstraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGenericPoolAllocator.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeometryUtil.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGhostObject.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkConvexCast.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkEpa2.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkEpaPenetrationDepthSolver.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkPairDetector.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btHeightfieldTerrainShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btHinge2Constraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btHingeConstraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btKinematicCharacterController.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btManifoldResult.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btMinkowskiPenetrationDepthSolver.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btMinkowskiSumShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultiSapBroadphase.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultiSphereShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultimaterialTriangleMeshShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btOptimizedBvh.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btOverlappingPairCache.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btPersistentManifold.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btPoint2PointConstraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btPolyhedralConvexShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btQuantizedBvh.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btQuickprof.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btRaycastCallback.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btRaycastVehicle.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btRigidBody.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btScaledBvhTriangleMeshShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSequentialImpulseConstraintSolver.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btShapeHull.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimpleBroadphase.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimpleDynamicsWorld.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimulationIslandManager.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSliderConstraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBody.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyConcaveCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyHelpers.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyRigidBodyCollisionConfiguration.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftRigidCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftRigidDynamicsWorld.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftSoftCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSolve2LinearConstraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereBoxCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereSphereCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereTriangleCollisionAlgorithm.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btStaticPlaneShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btStridingMeshInterface.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btSubSimplexConvexCast.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btTetrahedronShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleBuffer.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleCallback.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleIndexVertexArray.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleIndexVertexMaterialArray.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleMesh.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleMeshShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleShapeEx.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btTypedConstraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btUniformScalingShape.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btUnionFind.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btUniversalConstraint.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btVoronoiSimplexSolver.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/btWheelInfo.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_box_set.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_contact.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_memory.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_tri_collision.cpp
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btAxisSweep3.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDbvt.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDispatcher.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBoxBoxDetector.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionObject.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionWorld.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btGhostObject.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btManifoldResult.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btUnionFind.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBox2dShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBoxShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCapsuleShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCollisionMargin.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCollisionShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCompoundShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConcaveShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConeShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvex2dShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexHullShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexInternalShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexPointCloudShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCylinderShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btEmptyShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMaterial.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMultiSphereShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btOptimizedBvh.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btShapeHull.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btSphereShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btStaticPlaneShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btStridingMeshInterface.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTetrahedronShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleBuffer.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleCallback.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleMesh.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleMeshShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btUniformScalingShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btBoxCollision.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btClipPolygon.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btContactProcessing.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactBvh.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactMassUtil.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactQuantizedBvh.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactShape.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGenericPoolAllocator.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGeometryOperations.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btQuantization.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btTriangleShapeEx.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_array.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_basic_geometry_operations.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_bitset.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_box_collision.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_box_set.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_clip_polygon.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_contact.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_geom_types.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_geometry.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_hash_table.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_linear_math.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_math.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_memory.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_radixsort.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_tri_collision.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btConvexCast.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btPointCollector.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/btCharacterControllerInterface.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Character/btKinematicCharacterController.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btConstraintSolver.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btContactConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btHingeConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btJacobianEntry.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSliderConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolverBody.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btSolverConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btTypedConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btActionInterface.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btDynamicsWorld.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btRigidBody.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btRaycastVehicle.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btVehicleRaycaster.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletDynamics/Vehicle/btWheelInfo.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBody.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyHelpers.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyInternals.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftRigidCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftRigidDynamicsWorld.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSoftSoftCollisionAlgorithm.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletSoftBody/btSparseSDF.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAabbUtil2.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAlignedAllocator.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btAlignedObjectArray.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btConvexHull.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btDefaultMotionState.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btGeometryUtil.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btHashMap.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btIDebugDraw.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btList.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMatrix3x3.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMinMax.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btMotionState.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btPoolAllocator.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuadWord.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuaternion.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btQuickprof.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btRandom.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btScalar.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btStackAlloc.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btTransform.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btTransformUtil.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/LinearMath/btVector3.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/btBulletCollisionCommon.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/include/btBulletDynamicsCommon.h
test-suite/trunk/MultiSource/Benchmarks/Bullet/landscape.mdl
test-suite/trunk/MultiSource/Benchmarks/Bullet/main.cpp
Modified:
test-suite/trunk/MultiSource/Benchmarks/Makefile
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/BenchmarkDemo.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/BenchmarkDemo.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/BenchmarkDemo.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/BenchmarkDemo.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1251 @@
+/*
+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.
+*/
+
+// Collision Radius
+#define COLLISION_RADIUS 0.0f
+
+#include "BenchmarkDemo.h"
+#ifdef USE_GLUT_DEMO_APPLICATION
+#include "GlutStuff.h"
+#endif //USE_GLUT_DEMO_APPLICATION
+
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include <stdio.h> //printf debugging
+#include "Taru.mdl"
+#include "landscape.mdl"
+#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
+//#include "BulletMultiThreaded/SpuGatheringCollisionDispatcher.h"
+//#include "BulletMultiThreaded/SequentialThreadSupport.h"
+//#include "BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+
+
+
+
+class btRaycastBar2
+{
+public:
+ btVector3 source[NUMRAYS];
+ btVector3 dest[NUMRAYS];
+ btVector3 direction[NUMRAYS];
+ btVector3 hit[NUMRAYS];
+ btVector3 normal[NUMRAYS];
+
+ int frame_counter;
+ int ms;
+ int sum_ms;
+ int sum_ms_samples;
+ int min_ms;
+ int max_ms;
+
+#ifdef USE_BT_CLOCK
+ btClock frame_timer;
+#endif //USE_BT_CLOCK
+
+ btScalar dx;
+ btScalar min_x;
+ btScalar max_x;
+ btScalar max_y;
+ btScalar sign;
+
+ btRaycastBar2 ()
+ {
+ ms = 0;
+ max_ms = 0;
+ min_ms = 9999;
+ sum_ms_samples = 0;
+ sum_ms = 0;
+ }
+
+
+
+ btRaycastBar2 (btScalar ray_length, btScalar z,btScalar max_y)
+ {
+ frame_counter = 0;
+ ms = 0;
+ max_ms = 0;
+ min_ms = 9999;
+ sum_ms_samples = 0;
+ sum_ms = 0;
+ dx = 10.0;
+ min_x = 0;
+ max_x = 0;
+ this->max_y = max_y;
+ sign = 1.0;
+ btScalar dalpha = 2*SIMD_2_PI/NUMRAYS;
+ for (int i = 0; i < NUMRAYS; i++)
+ {
+ btScalar alpha = dalpha * i;
+ // rotate around by alpha degrees y
+ btQuaternion q(btVector3(0.0, 1.0, 0.0), alpha);
+ direction[i] = btVector3(1.0, 0.0, 0.0);
+ direction[i] = quatRotate(q , direction[i]);
+ direction[i] = direction[i] * ray_length;
+
+
+ source[i] = btVector3(min_x, max_y, z);
+ dest[i] = source[i] + direction[i];
+ dest[i][1]=-1000;
+ normal[i] = btVector3(1.0, 0.0, 0.0);
+ }
+ }
+
+ void move (btScalar dt)
+ {
+ if (dt > btScalar(1.0/60.0))
+ dt = btScalar(1.0/60.0);
+ for (int i = 0; i < NUMRAYS; i++)
+ {
+ source[i][0] += dx * dt * sign;
+ dest[i][0] += dx * dt * sign;
+ }
+ if (source[0][0] < min_x)
+ sign = 1.0;
+ else if (source[0][0] > max_x)
+ sign = -1.0;
+ }
+
+ void cast (btCollisionWorld* cw)
+ {
+#ifdef USE_BT_CLOCK
+ frame_timer.reset ();
+#endif //USE_BT_CLOCK
+
+#ifdef BATCH_RAYCASTER
+ if (!gBatchRaycaster)
+ return;
+
+ gBatchRaycaster->clearRays ();
+ for (int i = 0; i < NUMRAYS; i++)
+ {
+ gBatchRaycaster->addRay (source[i], dest[i]);
+ }
+ gBatchRaycaster->performBatchRaycast ();
+ for (int i = 0; i < gBatchRaycaster->getNumRays (); i++)
+ {
+ const SpuRaycastTaskWorkUnitOut& out = (*gBatchRaycaster)[i];
+ hit[i].setInterpolate3(source[i],dest[i],out.hitFraction);
+ normal[i] = out.hitNormal;
+ normal[i].normalize ();
+ }
+#else
+ for (int i = 0; i < NUMRAYS; i++)
+ {
+ btCollisionWorld::ClosestRayResultCallback cb(source[i], dest[i]);
+
+ cw->rayTest (source[i], dest[i], cb);
+ if (cb.hasHit ())
+ {
+ hit[i] = cb.m_hitPointWorld;
+ normal[i] = cb.m_hitNormalWorld;
+ normal[i].normalize ();
+ } else {
+ hit[i] = dest[i];
+ normal[i] = btVector3(1.0, 0.0, 0.0);
+ }
+
+ }
+#ifdef USE_BT_CLOCK
+ ms += frame_timer.getTimeMilliseconds ();
+#endif //USE_BT_CLOCK
+ frame_counter++;
+ if (frame_counter > 50)
+ {
+ min_ms = ms < min_ms ? ms : min_ms;
+ max_ms = ms > max_ms ? ms : max_ms;
+ sum_ms += ms;
+ sum_ms_samples++;
+ btScalar mean_ms = (btScalar)sum_ms/(btScalar)sum_ms_samples;
+ #ifndef NO_TIME
+ printf("%d rays in %d ms %d %d %f\n", NUMRAYS * frame_counter, ms, min_ms, max_ms, mean_ms);
+ #endif
+ ms = 0;
+ frame_counter = 0;
+ }
+#endif
+ }
+
+ void draw ()
+ {
+#ifdef USE_GLUT_DEMO_APPLICATION
+ glDisable (GL_LIGHTING);
+ glColor3f (0.0, 1.0, 0.0);
+ glBegin (GL_LINES);
+ int i;
+
+ for (i = 0; i < NUMRAYS; i++)
+ {
+ glVertex3f (source[i][0], source[i][1], source[i][2]);
+ glVertex3f (hit[i][0], hit[i][1], hit[i][2]);
+ }
+ glEnd ();
+ glColor3f (1.0, 1.0, 1.0);
+ glBegin (GL_LINES);
+ for (i = 0; i < NUMRAYS; i++)
+ {
+ glVertex3f (hit[i][0], hit[i][1], hit[i][2]);
+ glVertex3f (hit[i][0] + normal[i][0], hit[i][1] + normal[i][1], hit[i][2] + normal[i][2]);
+ }
+ glEnd ();
+ glColor3f (0.0, 1.0, 1.0);
+ glBegin (GL_POINTS);
+ for ( i = 0; i < NUMRAYS; i++)
+ {
+ glVertex3f (hit[i][0], hit[i][1], hit[i][2]);
+ }
+ glEnd ();
+ glEnable (GL_LIGHTING);
+#endif //USE_GLUT_DEMO_APPLICATION
+
+ }
+};
+
+
+static btRaycastBar2 raycastBar;
+
+
+void BenchmarkDemo::clientMoveAndDisplay()
+{
+#ifdef USE_GLUT_DEMO_APPLICATION
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+#endif //USE_GLUT_DEMO_APPLICATION
+
+ //simple dynamics world doesn't handle fixed-time-stepping
+ float ms = getDeltaTimeMicroseconds();
+
+ ///step the simulation
+ if (m_dynamicsWorld)
+ {
+ m_dynamicsWorld->stepSimulation(btScalar(1./60.));
+ //optional but useful: debug drawing
+ m_dynamicsWorld->debugDrawWorld();
+ }
+
+ if (m_benchmark==7)
+ {
+ castRays();
+
+ raycastBar.draw();
+
+ }
+
+ renderme();
+
+#ifdef USE_GLUT_DEMO_APPLICATION
+ glFlush();
+
+ glutSwapBuffers();
+#endif //USE_GLUT_DEMO_APPLICATION
+
+}
+
+
+
+void BenchmarkDemo::displayCallback(void)
+{
+
+#ifdef USE_GLUT_DEMO_APPLICATION
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ renderme();
+
+ //optional but useful: debug drawing to detect problems
+ if (m_dynamicsWorld)
+ m_dynamicsWorld->debugDrawWorld();
+
+ glFlush();
+ glutSwapBuffers();
+#endif //USE_GLUT_DEMO_APPLICATION
+}
+
+
+
+
+
+void BenchmarkDemo::initPhysics()
+{
+
+ setCameraDistance(btScalar(100.));
+
+ ///collision configuration contains default setup for memory, collision setup
+ m_collisionConfiguration = new btDefaultCollisionConfiguration();
+
+ ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+ m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+#if 0
+ SequentialThreadSupport::SequentialThreadConstructionInfo sci("spuCD",
+ processCollisionTask,
+ createCollisionLocalStoreMemory);
+
+ SequentialThreadSupport* seq = new SequentialThreadSupport(sci);
+ m_dispatcher = new SpuGatheringCollisionDispatcher(seq,1,m_collisionConfiguration);
+#endif
+
+
+ ///the maximum size of the collision world. Make sure objects stay within these boundaries
+ ///Don't make the world AABB size too large, it will harm simulation quality and performance
+ btVector3 worldAabbMin(-10000,-10000,-10000);
+ btVector3 worldAabbMax(10000,10000,10000);
+
+ btHashedOverlappingPairCache* pairCache = new btHashedOverlappingPairCache();
+ m_overlappingPairCache = new btAxisSweep3(worldAabbMin,worldAabbMax,3500,pairCache);
+// m_overlappingPairCache = new btSimpleBroadphase();
+// m_overlappingPairCache = new btDbvtBroadphase();
+
+
+
+ ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
+ btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
+
+ m_solver = sol;
+
+ btDiscreteDynamicsWorld* dynamicsWorld;
+ m_dynamicsWorld = dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver,m_collisionConfiguration);
+ //dynamicsWorld->getSimulationIslandManager()->setSplitIslands(false);
+ dynamicsWorld->getDispatchInfo().m_useConvexConservativeDistanceUtil = true;
+ dynamicsWorld->getDispatchInfo().m_convexConservativeDistanceThreshold = btScalar(0.01);
+
+
+ m_dynamicsWorld->setGravity(btVector3(0,-10,0));
+
+ if (m_benchmark<5)
+ {
+ ///create a few basic rigid bodies
+ // btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
+ btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),0);
+
+ m_collisionShapes.push_back(groundShape);
+
+ btTransform groundTransform;
+ groundTransform.setIdentity();
+ groundTransform.setOrigin(btVector3(0,0,0));
+
+ //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
+ {
+ btScalar mass(0.);
+
+ //rigidbody is dynamic if and only if mass is non zero, otherwise static
+ bool isDynamic = (mass != 0.f);
+
+ btVector3 localInertia(0,0,0);
+ if (isDynamic)
+ groundShape->calculateLocalInertia(mass,localInertia);
+
+ //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+ btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+ btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
+ btRigidBody* body = new btRigidBody(rbInfo);
+
+ //add the body to the dynamics world
+ m_dynamicsWorld->addRigidBody(body);
+ }
+ }
+
+ switch (m_benchmark)
+ {
+ case 1:
+ {
+ createTest1();
+ break;
+ }
+ case 2:
+ {
+ createTest2();
+ break;
+ }
+ case 3:
+ {
+ createTest3();
+ break;
+ }
+ case 4:
+ {
+ createTest4();
+ break;
+ }
+ case 5:
+ {
+ createTest5();
+ break;
+ }
+ case 6:
+ {
+ createTest6();
+ break;
+ }
+ case 7:
+ {
+ createTest7();
+ break;
+ }
+
+
+ default:
+ {
+ }
+ }
+
+
+ clientResetScene();
+}
+
+
+void BenchmarkDemo::createTest1()
+{
+ // 3000
+ int size = 8;
+ const float cubeSize = 1.0f;
+ float spacing = cubeSize;
+ btVector3 pos(0.0f, cubeSize * 2,0.f);
+ float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
+
+ btBoxShape* blockShape = new btBoxShape(btVector3(cubeSize-COLLISION_RADIUS,cubeSize-COLLISION_RADIUS,cubeSize-COLLISION_RADIUS));
+ btVector3 localInertia(0,0,0);
+ float mass = 2.f;
+ blockShape->calculateLocalInertia(mass,localInertia);
+
+ btTransform trans;
+ trans.setIdentity();
+
+ for(int k=0;k<47;k++) {
+ for(int j=0;j<size;j++) {
+ pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
+ for(int i=0;i<size;i++) {
+ pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
+
+ trans.setOrigin(pos);
+ btRigidBody* cmbody = localCreateRigidBody(mass,trans,blockShape);
+ }
+ }
+ offset -= 0.05f * spacing * (size-1);
+// spacing *= 1.01f;
+ pos[1] += (cubeSize * 2.0f + spacing);
+ }
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Pyramid 3
+
+void BenchmarkDemo::createWall(const btVector3& offsetPosition,int stackSize,const btVector3& boxSize)
+{
+
+ btBoxShape* blockShape = new btBoxShape(btVector3(boxSize[0]-COLLISION_RADIUS,boxSize[1]-COLLISION_RADIUS,boxSize[2]-COLLISION_RADIUS));
+
+ float mass = 1.f;
+ btVector3 localInertia(0,0,0);
+ blockShape->calculateLocalInertia(mass,localInertia);
+
+ float diffX = boxSize[0] * 1.0f;
+ float diffY = boxSize[1] * 1.0f;
+ float diffZ = boxSize[2] * 1.0f;
+
+ float offset = -stackSize * (diffZ * 2.0f) * 0.5f;
+ btVector3 pos(0.0f, diffY, 0.0f);
+
+ btTransform trans;
+ trans.setIdentity();
+
+ while(stackSize) {
+ for(int i=0;i<stackSize;i++) {
+ pos[2] = offset + (float)i * (diffZ * 2.0f);
+
+ trans.setOrigin(offsetPosition + pos);
+ localCreateRigidBody(mass,trans,blockShape);
+
+ }
+ offset += diffZ;
+ pos[1] += (diffY * 2.0f);
+ stackSize--;
+ }
+}
+
+void BenchmarkDemo::createPyramid(const btVector3& offsetPosition,int stackSize,const btVector3& boxSize)
+{
+
+ float space = 0.000f;
+ btVector3 pos(0.0f, boxSize[1], 0.0f);
+
+ btBoxShape* blockShape = new btBoxShape(btVector3(boxSize[0]-COLLISION_RADIUS,boxSize[1]-COLLISION_RADIUS,boxSize[2]-COLLISION_RADIUS));
+ btTransform trans;
+ trans.setIdentity();
+
+ float mass = 1.f;
+ btVector3 localInertia(0,0,0);
+ blockShape->calculateLocalInertia(mass,localInertia);
+
+
+ float diffX = boxSize[0];
+ float diffY = boxSize[1];
+ float diffZ = boxSize[2];
+
+ float offsetX = -stackSize * (diffX * 2.0f + space) * 0.5f;
+ float offsetZ = -stackSize * (diffZ * 2.0f + space) * 0.5f;
+ while(stackSize) {
+ for(int j=0;j<stackSize;j++) {
+ pos[2] = offsetZ + (float)j * (diffZ * 2.0f + space);
+ for(int i=0;i<stackSize;i++) {
+ pos[0] = offsetX + (float)i * (diffX * 2.0f + space);
+ trans.setOrigin(offsetPosition + pos);
+ this->localCreateRigidBody(mass,trans,blockShape);
+
+
+ }
+ }
+ offsetX += diffX;
+ offsetZ += diffZ;
+ pos[1] += (diffY * 2.0f + space);
+ stackSize--;
+ }
+
+}
+
+ const btVector3 rotate( const btQuaternion& quat, const btVector3 & vec )
+{
+ float tmpX, tmpY, tmpZ, tmpW;
+ tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
+ tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
+ tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
+ tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
+ return btVector3(
+ ( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
+ ( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
+ ( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
+ );
+}
+
+void BenchmarkDemo::createTowerCircle(const btVector3& offsetPosition,int stackSize,int rotSize,const btVector3& boxSize)
+{
+
+ btBoxShape* blockShape = new btBoxShape(btVector3(boxSize[0]-COLLISION_RADIUS,boxSize[1]-COLLISION_RADIUS,boxSize[2]-COLLISION_RADIUS));
+
+ btTransform trans;
+ trans.setIdentity();
+
+ float mass = 1.f;
+ btVector3 localInertia(0,0,0);
+ blockShape->calculateLocalInertia(mass,localInertia);
+
+
+ float radius = 1.3f * rotSize * boxSize[0] / SIMD_PI;
+
+ // create active boxes
+ btQuaternion rotY(0,1,0,0);
+ float posY = boxSize[1];
+
+ for(int i=0;i<stackSize;i++) {
+ for(int j=0;j<rotSize;j++) {
+
+
+ trans.setOrigin(offsetPosition+ rotate(rotY,btVector3(0.0f , posY, radius)));
+ trans.setRotation(rotY);
+ localCreateRigidBody(mass,trans,blockShape);
+
+ rotY *= btQuaternion(btVector3(0,1,0),SIMD_PI/(rotSize*btScalar(0.5)));
+ }
+
+ posY += boxSize[1] * 2.0f;
+ rotY *= btQuaternion(btVector3(0,1,0),SIMD_PI/(float)rotSize);
+ }
+
+}
+
+void BenchmarkDemo::createTest2()
+{
+ setCameraDistance(btScalar(50.));
+ const float cubeSize = 1.0f;
+
+ createPyramid(btVector3(-20.0f,0.0f,0.0f),12,btVector3(cubeSize,cubeSize,cubeSize));
+ createWall(btVector3(-2.0f,0.0f,0.0f),12,btVector3(cubeSize,cubeSize,cubeSize));
+ createWall(btVector3(4.0f,0.0f,0.0f),12,btVector3(cubeSize,cubeSize,cubeSize));
+ createWall(btVector3(10.0f,0.0f,0.0f),12,btVector3(cubeSize,cubeSize,cubeSize));
+ createTowerCircle(btVector3(25.0f,0.0f,0.0f),8,24,btVector3(cubeSize,cubeSize,cubeSize));
+
+}
+
+
+
+
+// Enrico: Shouldn't these three variables be real constants and not defines?
+
+#ifndef M_PI
+#define M_PI btScalar(3.14159265358979323846)
+#endif
+
+#ifndef M_PI_2
+#define M_PI_2 btScalar(1.57079632679489661923)
+#endif
+
+#ifndef M_PI_4
+#define M_PI_4 btScalar(0.785398163397448309616)
+#endif
+
+class RagDoll
+{
+ enum
+ {
+ BODYPART_PELVIS = 0,
+ BODYPART_SPINE,
+ BODYPART_HEAD,
+
+ BODYPART_LEFT_UPPER_LEG,
+ BODYPART_LEFT_LOWER_LEG,
+
+ BODYPART_RIGHT_UPPER_LEG,
+ BODYPART_RIGHT_LOWER_LEG,
+
+ BODYPART_LEFT_UPPER_ARM,
+ BODYPART_LEFT_LOWER_ARM,
+
+ BODYPART_RIGHT_UPPER_ARM,
+ BODYPART_RIGHT_LOWER_ARM,
+
+ BODYPART_COUNT
+ };
+
+ enum
+ {
+ JOINT_PELVIS_SPINE = 0,
+ JOINT_SPINE_HEAD,
+
+ JOINT_LEFT_HIP,
+ JOINT_LEFT_KNEE,
+
+ JOINT_RIGHT_HIP,
+ JOINT_RIGHT_KNEE,
+
+ JOINT_LEFT_SHOULDER,
+ JOINT_LEFT_ELBOW,
+
+ JOINT_RIGHT_SHOULDER,
+ JOINT_RIGHT_ELBOW,
+
+ JOINT_COUNT
+ };
+
+ btDynamicsWorld* m_ownerWorld;
+ btCollisionShape* m_shapes[BODYPART_COUNT];
+ btRigidBody* m_bodies[BODYPART_COUNT];
+ btTypedConstraint* m_joints[JOINT_COUNT];
+
+ btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
+ {
+ bool isDynamic = (mass != 0.f);
+
+ btVector3 localInertia(0,0,0);
+ if (isDynamic)
+ shape->calculateLocalInertia(mass,localInertia);
+
+ btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
+
+ btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
+ btRigidBody* body = new btRigidBody(rbInfo);
+
+ m_ownerWorld->addRigidBody(body);
+
+ return body;
+ }
+
+public:
+ RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset,btScalar scale)
+ : m_ownerWorld (ownerWorld)
+ {
+ // Setup the geometry
+ m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(0.15)*scale, btScalar(0.20)*scale);
+ m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(0.15)*scale, btScalar(0.28)*scale);
+ m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(0.10)*scale, btScalar(0.05)*scale);
+ m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07)*scale, btScalar(0.45)*scale);
+ m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05)*scale, btScalar(0.37)*scale);
+ m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07)*scale, btScalar(0.45)*scale);
+ m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05)*scale, btScalar(0.37)*scale);
+ m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05)*scale, btScalar(0.33)*scale);
+ m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04)*scale, btScalar(0.25)*scale);
+ m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05)*scale, btScalar(0.33)*scale);
+ m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04)*scale, btScalar(0.25)*scale);
+
+ // Setup all the rigid bodies
+ btTransform offset; offset.setIdentity();
+ offset.setOrigin(positionOffset);
+
+ btTransform transform;
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(0.), btScalar(1.), btScalar(0.)));
+ m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS]);
+
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(0.), btScalar(1.2), btScalar(0.)));
+ m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE]);
+
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(0.), btScalar(1.6), btScalar(0.)));
+ m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]);
+
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(-0.18), btScalar(0.65), btScalar(0.)));
+ m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]);
+
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(-0.18), btScalar(0.2), btScalar(0.)));
+ m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]);
+
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(0.18), btScalar(0.65), btScalar(0.)));
+ m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]);
+
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(0.18), btScalar(0.2), btScalar(0.)));
+ m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]);
+
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(-0.35), btScalar(1.45), btScalar(0.)));
+ transform.getBasis().setEulerZYX(0,0,M_PI_2);
+ m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]);
+
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(-0.7), btScalar(1.45), btScalar(0.)));
+ transform.getBasis().setEulerZYX(0,0,M_PI_2);
+ m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]);
+
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(0.35), btScalar(1.45), btScalar(0.)));
+ transform.getBasis().setEulerZYX(0,0,-M_PI_2);
+ m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]);
+
+ transform.setIdentity();
+ transform.setOrigin(scale*btVector3(btScalar(0.7), btScalar(1.45), btScalar(0.)));
+ transform.getBasis().setEulerZYX(0,0,-M_PI_2);
+ m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]);
+
+ // Setup some damping on the m_bodies
+ for (int i = 0; i < BODYPART_COUNT; ++i)
+ {
+ m_bodies[i]->setDamping(btScalar(0.05), btScalar(0.85));
+ m_bodies[i]->setDeactivationTime(btScalar(0.8));
+ m_bodies[i]->setSleepingThresholds(btScalar(1.6), btScalar(2.5));
+ }
+
+ // Now setup the constraints
+ btHingeConstraint* hingeC;
+ btConeTwistConstraint* coneC;
+
+ btTransform localA, localB;
+
+ localA.setIdentity(); localB.setIdentity();
+ localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.15), btScalar(0.)));
+ localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.15), btScalar(0.)));
+ hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB);
+ hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2));
+ m_joints[JOINT_PELVIS_SPINE] = hingeC;
+ m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true);
+
+
+ localA.setIdentity(); localB.setIdentity();
+ localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.30), btScalar(0.)));
+ localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
+ coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB);
+ coneC->setLimit(M_PI_4, M_PI_4, M_PI_2);
+ m_joints[JOINT_SPINE_HEAD] = coneC;
+ m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true);
+
+
+ localA.setIdentity(); localB.setIdentity();
+ localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(scale*btVector3(btScalar(-0.18), btScalar(-0.10), btScalar(0.)));
+ localB.getBasis().setEulerZYX(0,0,-M_PI_4*5); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.225), btScalar(0.)));
+ coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB);
+ coneC->setLimit(M_PI_4, M_PI_4, 0);
+ m_joints[JOINT_LEFT_HIP] = coneC;
+ m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true);
+
+ localA.setIdentity(); localB.setIdentity();
+ localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.)));
+ localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.185), btScalar(0.)));
+ hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB);
+ hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
+ m_joints[JOINT_LEFT_KNEE] = hingeC;
+ m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true);
+
+
+ localA.setIdentity(); localB.setIdentity();
+ localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(scale*btVector3(btScalar(0.18), btScalar(-0.10), btScalar(0.)));
+ localB.getBasis().setEulerZYX(0,0,M_PI_4); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.225), btScalar(0.)));
+ coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB);
+ coneC->setLimit(M_PI_4, M_PI_4, 0);
+ m_joints[JOINT_RIGHT_HIP] = coneC;
+ m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true);
+
+ localA.setIdentity(); localB.setIdentity();
+ localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.)));
+ localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.185), btScalar(0.)));
+ hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB);
+ hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
+ m_joints[JOINT_RIGHT_KNEE] = hingeC;
+ m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true);
+
+
+ localA.setIdentity(); localB.setIdentity();
+ localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(scale*btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.)));
+ localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.)));
+ coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB);
+ coneC->setLimit(M_PI_2, M_PI_2, 0);
+ m_joints[JOINT_LEFT_SHOULDER] = coneC;
+ m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true);
+
+ localA.setIdentity(); localB.setIdentity();
+ localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.18), btScalar(0.)));
+ localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
+ hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB);
+ hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
+ m_joints[JOINT_LEFT_ELBOW] = hingeC;
+ m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true);
+
+
+
+ localA.setIdentity(); localB.setIdentity();
+ localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(scale*btVector3(btScalar(0.2), btScalar(0.15), btScalar(0.)));
+ localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.)));
+ coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB);
+ coneC->setLimit(M_PI_2, M_PI_2, 0);
+ m_joints[JOINT_RIGHT_SHOULDER] = coneC;
+ m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true);
+
+ localA.setIdentity(); localB.setIdentity();
+ localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.18), btScalar(0.)));
+ localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
+ hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB);
+ hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
+ m_joints[JOINT_RIGHT_ELBOW] = hingeC;
+ m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true);
+ }
+
+ virtual ~RagDoll ()
+ {
+ int i;
+
+ // Remove all constraints
+ for ( i = 0; i < JOINT_COUNT; ++i)
+ {
+ m_ownerWorld->removeConstraint(m_joints[i]);
+ delete m_joints[i]; m_joints[i] = 0;
+ }
+
+ // Remove all bodies and shapes
+ for ( i = 0; i < BODYPART_COUNT; ++i)
+ {
+ m_ownerWorld->removeRigidBody(m_bodies[i]);
+
+ delete m_bodies[i]->getMotionState();
+
+ delete m_bodies[i]; m_bodies[i] = 0;
+ delete m_shapes[i]; m_shapes[i] = 0;
+ }
+ }
+};
+
+void BenchmarkDemo::createTest3()
+{
+ setCameraDistance(btScalar(50.));
+
+ int size = 16;
+
+ float sizeX = 1.f;
+ float sizeY = 1.f;
+
+ int rc=0;
+
+ btScalar scale(3.5);
+ btVector3 pos(0.0f, sizeY, 0.0f);
+ while(size) {
+ float offset = -size * (sizeX * 6.0f) * 0.5f;
+ for(int i=0;i<size;i++) {
+ pos[0] = offset + (float)i * (sizeX * 6.0f);
+
+ RagDoll* ragDoll = new RagDoll (m_dynamicsWorld,pos,scale);
+ m_ragdolls.push_back(ragDoll);
+ }
+
+ offset += sizeX;
+ pos[1] += (sizeY * 7.0f);
+ pos[2] -= sizeX * 2.0f;
+ size--;
+ }
+
+}
+void BenchmarkDemo::createTest4()
+{
+ setCameraDistance(btScalar(50.));
+
+ int size = 8;
+ const float cubeSize = 1.5f;
+ float spacing = cubeSize;
+ btVector3 pos(0.0f, cubeSize * 2, 0.0f);
+ float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
+
+ btConvexHullShape* convexHullShape = new btConvexHullShape();
+
+ btScalar scaling(1);
+
+ convexHullShape->setLocalScaling(btVector3(scaling,scaling,scaling));
+
+ for (int i=0;i<TaruVtxCount;i++)
+ {
+ btVector3 vtx(TaruVtx[i*3],TaruVtx[i*3+1],TaruVtx[i*3+2]);
+ convexHullShape->addPoint(vtx*btScalar(1./scaling));
+ }
+
+ btTransform trans;
+ trans.setIdentity();
+
+ float mass = 1.f;
+ btVector3 localInertia(0,0,0);
+ convexHullShape->calculateLocalInertia(mass,localInertia);
+
+ for(int k=0;k<15;k++) {
+ for(int j=0;j<size;j++) {
+ pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
+ for(int i=0;i<size;i++) {
+ pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
+ trans.setOrigin(pos);
+ localCreateRigidBody(mass,trans,convexHullShape);
+ }
+ }
+ offset -= 0.05f * spacing * (size-1);
+ spacing *= 1.01f;
+ pos[1] += (cubeSize * 2.0f + spacing);
+ }
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+// LargeMesh
+
+int LandscapeVtxCount[] = {
+ Landscape01VtxCount,
+ Landscape02VtxCount,
+ Landscape03VtxCount,
+ Landscape04VtxCount,
+ Landscape05VtxCount,
+ Landscape06VtxCount,
+ Landscape07VtxCount,
+ Landscape08VtxCount,
+};
+
+int LandscapeIdxCount[] = {
+ Landscape01IdxCount,
+ Landscape02IdxCount,
+ Landscape03IdxCount,
+ Landscape04IdxCount,
+ Landscape05IdxCount,
+ Landscape06IdxCount,
+ Landscape07IdxCount,
+ Landscape08IdxCount,
+};
+
+btScalar *LandscapeVtx[] = {
+ Landscape01Vtx,
+ Landscape02Vtx,
+ Landscape03Vtx,
+ Landscape04Vtx,
+ Landscape05Vtx,
+ Landscape06Vtx,
+ Landscape07Vtx,
+ Landscape08Vtx,
+};
+
+btScalar *LandscapeNml[] = {
+ Landscape01Nml,
+ Landscape02Nml,
+ Landscape03Nml,
+ Landscape04Nml,
+ Landscape05Nml,
+ Landscape06Nml,
+ Landscape07Nml,
+ Landscape08Nml,
+};
+
+btScalar* LandscapeTex[] = {
+ Landscape01Tex,
+ Landscape02Tex,
+ Landscape03Tex,
+ Landscape04Tex,
+ Landscape05Tex,
+ Landscape06Tex,
+ Landscape07Tex,
+ Landscape08Tex,
+};
+
+unsigned short *LandscapeIdx[] = {
+ Landscape01Idx,
+ Landscape02Idx,
+ Landscape03Idx,
+ Landscape04Idx,
+ Landscape05Idx,
+ Landscape06Idx,
+ Landscape07Idx,
+ Landscape08Idx,
+};
+
+void BenchmarkDemo::createLargeMeshBody()
+{
+ btTransform trans;
+ trans.setIdentity();
+
+ for(int i=0;i<8;i++) {
+
+ btTriangleIndexVertexArray* meshInterface = new btTriangleIndexVertexArray();
+ btIndexedMesh part;
+
+ part.m_vertexBase = (const unsigned char*)LandscapeVtx[i];
+ part.m_vertexStride = sizeof(btScalar) * 3;
+ part.m_numVertices = LandscapeVtxCount[i];
+ part.m_triangleIndexBase = (const unsigned char*)LandscapeIdx[i];
+ part.m_triangleIndexStride = sizeof( short) * 3;
+ part.m_numTriangles = LandscapeIdxCount[i]/3;
+ part.m_indexType = PHY_SHORT;
+
+ meshInterface->addIndexedMesh(part,PHY_SHORT);
+
+ bool useQuantizedAabbCompression = true;
+ btBvhTriangleMeshShape* trimeshShape = new btBvhTriangleMeshShape(meshInterface,useQuantizedAabbCompression);
+ btVector3 localInertia(0,0,0);
+ trans.setOrigin(btVector3(0,-25,0));
+
+ btRigidBody* body = localCreateRigidBody(0,trans,trimeshShape);
+ body->setFriction (btScalar(0.9));
+
+ }
+
+}
+
+
+void BenchmarkDemo::createTest5()
+{
+ setCameraDistance(btScalar(250.));
+ btVector3 boxSize(1.5f,1.5f,1.5f);
+ float boxMass = 1.0f;
+ float sphereRadius = 1.5f;
+ float sphereMass = 1.0f;
+ float capsuleHalf = 2.0f;
+ float capsuleRadius = 1.0f;
+ float capsuleMass = 1.0f;
+
+ {
+ int size = 10;
+ int height = 10;
+
+ const float cubeSize = boxSize[0];
+ float spacing = 2.0f;
+ btVector3 pos(0.0f, 20.0f, 0.0f);
+ float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
+
+ int numBodies = 0;
+
+ for(int k=0;k<height;k++) {
+ for(int j=0;j<size;j++) {
+ pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
+ for(int i=0;i<size;i++) {
+ pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
+ btVector3 bpos = btVector3(0,25,0) + btVector3(5.0f,1.0f,5.0f)*pos;
+ int idx = rand() % 9;
+ btTransform trans;
+ trans.setIdentity();
+ trans.setOrigin(bpos);
+
+ switch(idx) {
+ case 0:case 1:case 2:
+ {
+ float r = 0.5f * (idx+1);
+ btBoxShape* boxShape = new btBoxShape(boxSize*r);
+ localCreateRigidBody(boxMass*r,trans,boxShape);
+ }
+ break;
+
+ case 3:case 4:case 5:
+ {
+ float r = 0.5f * (idx-3+1);
+ btSphereShape* sphereShape = new btSphereShape(sphereRadius*r);
+ localCreateRigidBody(sphereMass*r,trans,sphereShape);
+ }
+ break;
+
+ case 6:case 7:case 8:
+ {
+ float r = 0.5f * (idx-6+1);
+ btCapsuleShape* capsuleShape = new btCapsuleShape(capsuleRadius*r,capsuleHalf*r);
+ localCreateRigidBody(capsuleMass*r,trans,capsuleShape);
+ }
+ break;
+ }
+
+ numBodies++;
+ }
+ }
+ offset -= 0.05f * spacing * (size-1);
+ spacing *= 1.1f;
+ pos[1] += (cubeSize * 2.0f + spacing);
+ }
+ }
+
+ createLargeMeshBody();
+}
+void BenchmarkDemo::createTest6()
+{
+ setCameraDistance(btScalar(250.));
+
+ btVector3 boxSize(1.5f,1.5f,1.5f);
+
+ btConvexHullShape* convexHullShape = new btConvexHullShape();
+
+ for (int i=0;i<TaruVtxCount;i++)
+ {
+ btVector3 vtx(TaruVtx[i*3],TaruVtx[i*3+1],TaruVtx[i*3+2]);
+ convexHullShape->addPoint(vtx);
+ }
+
+ btTransform trans;
+ trans.setIdentity();
+
+ float mass = 1.f;
+ btVector3 localInertia(0,0,0);
+ convexHullShape->calculateLocalInertia(mass,localInertia);
+
+
+ {
+ int size = 10;
+ int height = 10;
+
+ const float cubeSize = boxSize[0];
+ float spacing = 2.0f;
+ btVector3 pos(0.0f, 20.0f, 0.0f);
+ float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
+
+
+ for(int k=0;k<height;k++) {
+ for(int j=0;j<size;j++) {
+ pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
+ for(int i=0;i<size;i++) {
+ pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
+ btVector3 bpos = btVector3(0,25,0) + btVector3(5.0f,1.0f,5.0f)*pos;
+ trans.setOrigin(bpos);
+
+ localCreateRigidBody(mass,trans,convexHullShape);
+ }
+ }
+ offset -= 0.05f * spacing * (size-1);
+ spacing *= 1.1f;
+ pos[1] += (cubeSize * 2.0f + spacing);
+ }
+ }
+
+
+ createLargeMeshBody();
+}
+
+
+
+
+void BenchmarkDemo::initRays()
+{
+ raycastBar = btRaycastBar2 (2500.0, 0,50.0);
+}
+
+void BenchmarkDemo::castRays()
+{
+ raycastBar.cast (m_dynamicsWorld);
+}
+
+void BenchmarkDemo::createTest7()
+{
+
+ createTest6();
+ setCameraDistance(btScalar(150.));
+ initRays();
+}
+
+void BenchmarkDemo::exitPhysics()
+{
+ int i;
+
+ for (i=0;i<m_ragdolls.size();i++)
+ {
+ RagDoll* doll = m_ragdolls[i];
+ delete doll;
+ }
+
+ //cleanup in the reverse order of creation/initialization
+
+ //remove the rigidbodies from the dynamics world and delete them
+ for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
+ {
+ btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+ btRigidBody* body = btRigidBody::upcast(obj);
+ if (body && body->getMotionState())
+ {
+ delete body->getMotionState();
+ }
+ m_dynamicsWorld->removeCollisionObject( obj );
+ delete obj;
+ }
+
+ //delete collision shapes
+ for (int j=0;j<m_collisionShapes.size();j++)
+ {
+ btCollisionShape* shape = m_collisionShapes[j];
+ delete shape;
+ }
+
+ //delete dynamics world
+ delete m_dynamicsWorld;
+
+ //delete solver
+ delete m_solver;
+
+ //delete broadphase
+ delete m_overlappingPairCache;
+
+ //delete dispatcher
+ delete m_dispatcher;
+
+ delete m_collisionConfiguration;
+
+
+}
+
+
+
+#ifndef USE_GLUT_DEMO_APPLICATION
+btRigidBody* DemoApplication::localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape)
+{
+ btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
+
+ //rigidbody is dynamic if and only if mass is non zero, otherwise static
+ bool isDynamic = (mass != 0.f);
+
+ btVector3 localInertia(0,0,0);
+ if (isDynamic)
+ shape->calculateLocalInertia(mass,localInertia);
+
+ //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+
+ btRigidBody* body = new btRigidBody(mass,0,shape,localInertia);
+ body->setWorldTransform(startTransform);
+
+ m_dynamicsWorld->addRigidBody(body);
+
+ return body;
+}
+#endif //USE_GLUT_DEMO_APPLICATION
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/BenchmarkDemo.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/BenchmarkDemo.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/BenchmarkDemo.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/BenchmarkDemo.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,250 @@
+/*
+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 BENCHMARK_DEMO_H
+#define BENCHMARK_DEMO_H
+
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+
+class btDynamicsWorld;
+
+#define NUMRAYS 500
+
+class btRigidBody;
+class btBroadphaseInterface;
+class btCollisionShape;
+class btOverlappingPairCache;
+class btCollisionDispatcher;
+class btConstraintSolver;
+struct btCollisionAlgorithmCreateFunc;
+class btDefaultCollisionConfiguration;
+
+#ifndef USE_GLUT_DEMO_APPLICATION
+///empty placeholder
+class DemoApplication
+{
+protected:
+
+ btDynamicsWorld* m_dynamicsWorld;
+public:
+ virtual void myinit() {}
+ virtual btDynamicsWorld* getDynamicsWorld()
+ {
+ return m_dynamicsWorld;
+ }
+
+ btScalar getDeltaTimeMicroseconds()
+ {
+ return 1.f;
+ }
+
+ void renderme() {}
+ void setCameraDistance(btScalar dist){}
+ void clientResetScene(){}
+ btRigidBody* localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape);
+
+};
+///BenchmarkDemo is provides several performance tests
+class BenchmarkDemo : public DemoApplication
+#else
+#include "GlutDemoApplication.h"
+class BenchmarkDemo : public GlutDemoApplication
+#endif
+
+
+{
+
+ //keep the collision shapes, for deletion/cleanup
+ btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
+
+ btAlignedObjectArray<class RagDoll*> m_ragdolls;
+
+ btBroadphaseInterface* m_overlappingPairCache;
+
+ btCollisionDispatcher* m_dispatcher;
+
+ btConstraintSolver* m_solver;
+
+ btDefaultCollisionConfiguration* m_collisionConfiguration;
+
+ int m_benchmark;
+
+ void createTest1();
+ void createTest2();
+ void createTest3();
+ void createTest4();
+ void createTest5();
+ void createTest6();
+ void createTest7();
+
+ void createWall(const btVector3& offsetPosition,int stackSize,const btVector3& boxSize);
+ void createPyramid(const btVector3& offsetPosition,int stackSize,const btVector3& boxSize);
+ void createTowerCircle(const btVector3& offsetPosition,int stackSize,int rotSize,const btVector3& boxSize);
+ void createLargeMeshBody();
+
+
+ class SpuBatchRaycaster* m_batchRaycaster;
+ class btThreadSupportInterface* m_batchRaycasterThreadSupport;
+
+ void castRays();
+ void initRays();
+
+ public:
+
+ BenchmarkDemo(int benchmark)
+ :m_benchmark(benchmark)
+ {
+ }
+ virtual ~BenchmarkDemo()
+ {
+ exitPhysics();
+ }
+ void initPhysics();
+
+ void exitPhysics();
+
+ virtual void clientMoveAndDisplay();
+
+ virtual void displayCallback();
+
+
+
+
+};
+
+class BenchmarkDemo1 : public BenchmarkDemo
+{
+public:
+ BenchmarkDemo1()
+ :BenchmarkDemo(1)
+ {
+ }
+
+ static DemoApplication* Create()
+ {
+ BenchmarkDemo1* demo = new BenchmarkDemo1;
+ demo->myinit();
+ demo->initPhysics();
+ return demo;
+ }
+};
+
+class BenchmarkDemo2 : public BenchmarkDemo
+{
+public:
+ BenchmarkDemo2()
+ :BenchmarkDemo(2)
+ {
+ }
+
+ static DemoApplication* Create()
+ {
+ BenchmarkDemo2* demo = new BenchmarkDemo2;
+ demo->myinit();
+ demo->initPhysics();
+ return demo;
+ }
+};
+
+class BenchmarkDemo3 : public BenchmarkDemo
+{
+public:
+ BenchmarkDemo3()
+ :BenchmarkDemo(3)
+ {
+ }
+
+ static DemoApplication* Create()
+ {
+ BenchmarkDemo3* demo = new BenchmarkDemo3;
+ demo->myinit();
+ demo->initPhysics();
+ return demo;
+ }
+};
+
+class BenchmarkDemo4 : public BenchmarkDemo
+{
+public:
+ BenchmarkDemo4()
+ :BenchmarkDemo(4)
+ {
+ }
+
+ static DemoApplication* Create()
+ {
+ BenchmarkDemo4* demo = new BenchmarkDemo4;
+ demo->myinit();
+ demo->initPhysics();
+ return demo;
+ }
+};
+
+
+class BenchmarkDemo5 : public BenchmarkDemo
+{
+public:
+ BenchmarkDemo5()
+ :BenchmarkDemo(5)
+ {
+ }
+
+ static DemoApplication* Create()
+ {
+ BenchmarkDemo5* demo = new BenchmarkDemo5;
+ demo->myinit();
+ demo->initPhysics();
+ return demo;
+ }
+};
+
+
+class BenchmarkDemo6 : public BenchmarkDemo
+{
+public:
+ BenchmarkDemo6()
+ :BenchmarkDemo(6)
+ {
+ }
+
+ static DemoApplication* Create()
+ {
+ BenchmarkDemo6* demo = new BenchmarkDemo6;
+ demo->myinit();
+ demo->initPhysics();
+ return demo;
+ }
+};
+
+class BenchmarkDemo7 : public BenchmarkDemo
+{
+public:
+ BenchmarkDemo7()
+ :BenchmarkDemo(7)
+ {
+ }
+
+ static DemoApplication* Create()
+ {
+ BenchmarkDemo7* demo = new BenchmarkDemo7;
+ demo->myinit();
+ demo->initPhysics();
+ return demo;
+ }
+};
+
+#endif //BENCHMARK_DEMO_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/Makefile
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/Makefile?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/Makefile (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/Makefile Sat Dec 19 14:05:59 2009
@@ -0,0 +1,8 @@
+LEVEL = ../../../
+PROG = bullet
+CPPFLAGS += -Iinclude -DNO_TIME
+LDFLAGS = -lstdc++
+
+include $(LEVEL)/Makefile.config
+
+include ../../Makefile.multisrc
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/SphereTriangleDetector.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/SphereTriangleDetector.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/SphereTriangleDetector.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/SphereTriangleDetector.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,209 @@
+/*
+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 "LinearMath/btScalar.h"
+#include "BulletCollision/CollisionDispatch/SphereTriangleDetector.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+
+SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
+:m_sphere(sphere),
+m_triangle(triangle),
+m_contactBreakingThreshold(contactBreakingThreshold)
+{
+
+}
+
+void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+{
+
+ (void)debugDraw;
+ const btTransform& transformA = input.m_transformA;
+ const btTransform& transformB = input.m_transformB;
+
+ btVector3 point,normal;
+ btScalar timeOfImpact = btScalar(1.);
+ btScalar depth = btScalar(0.);
+// output.m_distance = btScalar(BT_LARGE_FLOAT);
+ //move sphere into triangle space
+ btTransform sphereInTr = transformB.inverseTimes(transformA);
+
+ if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
+ {
+ if (swapResults)
+ {
+ btVector3 normalOnB = transformB.getBasis()*normal;
+ btVector3 normalOnA = -normalOnB;
+ btVector3 pointOnA = transformB*point+normalOnB*depth;
+ output.addContactPoint(normalOnA,pointOnA,depth);
+ } else
+ {
+ output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
+ }
+ }
+
+}
+
+#define MAX_OVERLAP btScalar(0.)
+
+
+
+// See also geometrictools.com
+// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
+
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
+ btVector3 diff = p - from;
+ btVector3 v = to - from;
+ btScalar t = v.dot(diff);
+
+ if (t > 0) {
+ btScalar dotVV = v.dot(v);
+ if (t < dotVV) {
+ t /= dotVV;
+ diff -= t*v;
+ } else {
+ t = 1;
+ diff -= v;
+ }
+ } else
+ t = 0;
+
+ nearest = from + t*v;
+ return diff.dot(diff);
+}
+
+bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) {
+ btVector3 lp(p);
+ btVector3 lnormal(normal);
+
+ return pointInTriangle(vertices, lnormal, &lp);
+}
+
+///combined discrete/continuous sphere-triangle
+bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
+{
+
+ const btVector3* vertices = &m_triangle->getVertexPtr(0);
+ const btVector3& c = sphereCenter;
+ btScalar r = m_sphere->getRadius();
+
+ btVector3 delta (0,0,0);
+
+ btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
+ normal.normalize();
+ btVector3 p1ToCentre = c - vertices[0];
+ btScalar distanceFromPlane = p1ToCentre.dot(normal);
+
+ if (distanceFromPlane < btScalar(0.))
+ {
+ //triangle facing the other way
+
+ distanceFromPlane *= btScalar(-1.);
+ normal *= btScalar(-1.);
+ }
+
+ btScalar contactMargin = contactBreakingThreshold;
+ bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
+ bool isInsideShellPlane = distanceFromPlane < r;
+
+ btScalar deltaDotNormal = delta.dot(normal);
+ if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
+ return false;
+
+ // Check for contact / intersection
+ bool hasContact = false;
+ btVector3 contactPoint;
+ if (isInsideContactPlane) {
+ if (facecontains(c,vertices,normal)) {
+ // Inside the contact wedge - touches a point on the shell plane
+ hasContact = true;
+ contactPoint = c - normal*distanceFromPlane;
+ } else {
+ // Could be inside one of the contact capsules
+ btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
+ btVector3 nearestOnEdge;
+ for (int i = 0; i < m_triangle->getNumEdges(); i++) {
+
+ btVector3 pa;
+ btVector3 pb;
+
+ m_triangle->getEdge(i,pa,pb);
+
+ btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
+ if (distanceSqr < contactCapsuleRadiusSqr) {
+ // Yep, we're inside a capsule
+ hasContact = true;
+ contactPoint = nearestOnEdge;
+ }
+
+ }
+ }
+ }
+
+ if (hasContact) {
+ btVector3 contactToCentre = c - contactPoint;
+ btScalar distanceSqr = contactToCentre.length2();
+ if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
+ btScalar distance = btSqrt(distanceSqr);
+ resultNormal = contactToCentre;
+ resultNormal.normalize();
+ point = contactPoint;
+ depth = -(r-distance);
+ return true;
+ }
+
+ if (delta.dot(contactToCentre) >= btScalar(0.0))
+ return false;
+
+ // Moving towards the contact point -> collision
+ point = contactPoint;
+ timeOfImpact = btScalar(0.0);
+ return true;
+ }
+
+ return false;
+}
+
+
+bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
+{
+ const btVector3* p1 = &vertices[0];
+ const btVector3* p2 = &vertices[1];
+ const btVector3* p3 = &vertices[2];
+
+ btVector3 edge1( *p2 - *p1 );
+ btVector3 edge2( *p3 - *p2 );
+ btVector3 edge3( *p1 - *p3 );
+
+ btVector3 p1_to_p( *p - *p1 );
+ btVector3 p2_to_p( *p - *p2 );
+ btVector3 p3_to_p( *p - *p3 );
+
+ btVector3 edge1_normal( edge1.cross(normal));
+ btVector3 edge2_normal( edge2.cross(normal));
+ btVector3 edge3_normal( edge3.cross(normal));
+
+ btScalar r1, r2, r3;
+ r1 = edge1_normal.dot( p1_to_p );
+ r2 = edge2_normal.dot( p2_to_p );
+ r3 = edge3_normal.dot( p3_to_p );
+ if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
+ ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
+ return true;
+ return false;
+
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/Taru.mdl
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/Taru.mdl?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/Taru.mdl (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/Taru.mdl Sat Dec 19 14:05:59 2009
@@ -0,0 +1,188 @@
+#define TaruVtxCount 43
+#define TaruIdxCount 132
+
+static float TaruVtx[] = {
+1.08664f,-1.99237f,0.0f,
+0.768369f,-1.99237f,-0.768369f,
+1.28852f,1.34412e-007f,-1.28852f,
+1.82224f,1.90735e-007f,0.0f,
+0.0f,-1.99237f,-1.08664f,
+0.0f,0.0f,-1.82224f,
+0.0f,-1.99237f,-1.08664f,
+-0.768369f,-1.99237f,-0.768369f,
+-1.28852f,1.34412e-007f,-1.28852f,
+0.0f,0.0f,-1.82224f,
+-1.08664f,-1.99237f,1.82086e-007f,
+-1.82224f,1.90735e-007f,1.59305e-007f,
+-0.768369f,-1.99237f,0.76837f,
+-1.28852f,2.47058e-007f,1.28852f,
+1.42495e-007f,-1.99237f,1.08664f,
+2.38958e-007f,2.70388e-007f,1.82224f,
+0.768369f,-1.99237f,0.768369f,
+1.28852f,2.47058e-007f,1.28852f,
+0.768369f,1.99237f,-0.768369f,
+1.08664f,1.99237f,0.0f,
+0.0f,1.99237f,-1.08664f,
+-0.768369f,1.99237f,-0.768369f,
+0.0f,1.99237f,-1.08664f,
+-1.08664f,1.99237f,0.0f,
+-0.768369f,1.99237f,0.768369f,
+1.42495e-007f,1.99237f,1.08664f,
+0.768369f,1.99237f,0.768369f,
+1.42495e-007f,-1.99237f,1.08664f,
+-0.768369f,-1.99237f,0.76837f,
+-1.08664f,-1.99237f,1.82086e-007f,
+-0.768369f,-1.99237f,-0.768369f,
+0.0f,-1.99237f,-1.08664f,
+0.768369f,-1.99237f,-0.768369f,
+1.08664f,-1.99237f,0.0f,
+0.768369f,-1.99237f,0.768369f,
+0.768369f,1.99237f,-0.768369f,
+0.0f,1.99237f,-1.08664f,
+-0.768369f,1.99237f,-0.768369f,
+-1.08664f,1.99237f,0.0f,
+-0.768369f,1.99237f,0.768369f,
+1.42495e-007f,1.99237f,1.08664f,
+0.768369f,1.99237f,0.768369f,
+1.08664f,1.99237f,0.0f,
+};
+
+static float TaruNml[] = {
+0.938103f,-0.346356f,0.0f,
+0.663339f,-0.346356f,-0.663339f,
+0.707107f,0.0f,-0.707107f,
+1.0f,0.0f,0.0f,
+0.0f,-0.346356f,-0.938103f,
+0.0f,0.0f,-1.0f,
+0.0f,-0.346356f,-0.938103f,
+-0.663339f,-0.346356f,-0.663339f,
+-0.707107f,0.0f,-0.707107f,
+0.0f,0.0f,-1.0f,
+-0.938103f,-0.346356f,0.0f,
+-1.0f,0.0f,0.0f,
+-0.663339f,-0.346356f,0.663339f,
+-0.707107f,0.0f,0.707107f,
+0.0f,-0.346356f,0.938103f,
+1.21445e-007f,0.0f,1.0f,
+0.663339f,-0.346356f,0.663339f,
+0.707107f,0.0f,0.707107f,
+0.663339f,0.346356f,-0.663339f,
+0.938103f,0.346356f,0.0f,
+0.0f,0.346356f,-0.938103f,
+-0.663339f,0.346356f,-0.663339f,
+0.0f,0.346356f,-0.938103f,
+-0.938103f,0.346356f,0.0f,
+-0.663339f,0.346356f,0.663339f,
+1.2508e-007f,0.346356f,0.938103f,
+0.663339f,0.346356f,0.663339f,
+0.0f,-1.0f,0.0f,
+0.0f,-1.0f,0.0f,
+0.0f,-1.0f,0.0f,
+0.0f,-1.0f,0.0f,
+0.0f,-1.0f,0.0f,
+0.0f,-1.0f,0.0f,
+0.0f,-1.0f,0.0f,
+0.0f,-1.0f,0.0f,
+0.0f,1.0f,0.0f,
+0.0f,1.0f,0.0f,
+0.0f,1.0f,0.0f,
+0.0f,1.0f,0.0f,
+0.0f,1.0f,0.0f,
+0.0f,1.0f,0.0f,
+0.0f,1.0f,0.0f,
+0.0f,1.0f,0.0f,
+};
+
+static float TaruTex[] = {
+0.75f,0.0f,
+0.875f,0.0f,
+0.875f,0.5f,
+0.75f,0.5f,
+1.0f,0.0f,
+1.0f,0.5f,
+0.0f,0.0f,
+0.125f,0.0f,
+0.125f,0.5f,
+0.0f,0.5f,
+0.25f,0.0f,
+0.25f,0.5f,
+0.375f,0.0f,
+0.375f,0.5f,
+0.5f,0.0f,
+0.5f,0.5f,
+0.625f,0.0f,
+0.625f,0.5f,
+0.875f,1.0f,
+0.75f,1.0f,
+1.0f,1.0f,
+0.125f,1.0f,
+0.0f,1.0f,
+0.25f,1.0f,
+0.375f,1.0f,
+0.5f,1.0f,
+0.625f,1.0f,
+1.0f,0.5f,
+0.853553f,0.146447f,
+0.5f,0.0f,
+0.146447f,0.146447f,
+0.0f,0.5f,
+0.146447f,0.853553f,
+0.5f,1.0f,
+0.853553f,0.853553f,
+0.146447f,0.853553f,
+0.0f,0.5f,
+0.146447f,0.146447f,
+0.5f,0.0f,
+0.853553f,0.146447f,
+1.0f,0.5f,
+0.853553f,0.853553f,
+0.5f,1.0f,
+};
+
+static unsigned short TaruIdx[] = {
+0,1,2,
+2,3,0,
+1,4,5,
+5,2,1,
+6,7,8,
+8,9,6,
+7,10,11,
+11,8,7,
+10,12,13,
+13,11,10,
+12,14,15,
+15,13,12,
+14,16,17,
+17,15,14,
+16,0,3,
+3,17,16,
+3,2,18,
+18,19,3,
+2,5,20,
+20,18,2,
+9,8,21,
+21,22,9,
+8,11,23,
+23,21,8,
+11,13,24,
+24,23,11,
+13,15,25,
+25,24,13,
+15,17,26,
+26,25,15,
+17,3,19,
+19,26,17,
+27,28,29,
+29,30,31,
+31,32,33,
+29,31,33,
+27,29,33,
+34,27,33,
+35,36,37,
+37,38,39,
+39,40,41,
+37,39,41,
+35,37,41,
+42,35,41,
+};
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btActivatingCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btActivatingCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btActivatingCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btActivatingCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -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 "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionDispatch/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();
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btAlignedAllocator.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btAlignedAllocator.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btAlignedAllocator.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btAlignedAllocator.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,205 @@
+/*
+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 "LinearMath/btAlignedAllocator.h"
+
+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 <malloc.h>
+static void *btAlignedAllocDefault(size_t size, int alignment)
+{
+ return _aligned_malloc(size, (size_t)alignment);
+}
+
+static void btAlignedFreeDefault(void *ptr)
+{
+ _aligned_free(ptr);
+}
+#elif defined(__CELLOS_LV2__)
+#include <stdlib.h>
+
+static inline void *btAlignedAllocDefault(size_t size, int alignment)
+{
+ return memalign(alignment, size);
+}
+
+static inline void btAlignedFreeDefault(void *ptr)
+{
+ free(ptr);
+}
+#else
+static inline void *btAlignedAllocDefault(size_t size, int alignment)
+{
+ void *ret;
+ char *real;
+ unsigned long offset;
+
+ 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);
+ *((void **)(ret)-1) = (void *)(real);
+ } else {
+ ret = (void *)(real);
+ }
+ return (ret);
+}
+
+static inline void btAlignedFreeDefault(void *ptr)
+{
+ void* real;
+
+ if (ptr) {
+ real = *((void **)(ptr)-1);
+ sFreeFunc(real);
+ }
+}
+#endif
+
+
+static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault;
+static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault;
+
+void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc)
+{
+ sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault;
+ sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault;
+}
+
+void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc)
+{
+ sAllocFunc = allocFunc ? allocFunc : btAllocDefault;
+ sFreeFunc = freeFunc ? freeFunc : btFreeDefault;
+}
+
+#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
+//this generic allocator provides the total allocated number of bytes
+#include <stdio.h>
+
+void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename)
+{
+ void *ret;
+ char *real;
+ unsigned long offset;
+
+ gTotalBytesAlignedAllocs += size;
+ gNumAlignedAllocs++;
+
+
+ real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1));
+ if (real) {
+ offset = (alignment - (unsigned long)(real + 2*sizeof(void *))) &
+(alignment-1);
+ ret = (void *)((real + 2*sizeof(void *)) + offset);
+ *((void **)(ret)-1) = (void *)(real);
+ *((int*)(ret)-2) = size;
+
+ } else {
+ ret = (void *)(real);//??
+ }
+
+ printf("allocation#%d at address %x, from %s,line %d, size %d\n",gNumAlignedAllocs,real, filename,line,size);
+
+ int* ptr = (int*)ret;
+ *ptr = 12;
+ return (ret);
+}
+
+void btAlignedFreeInternal (void* ptr,int line,char* filename)
+{
+
+ void* real;
+ gNumAlignedFree++;
+
+ if (ptr) {
+ real = *((void **)(ptr)-1);
+ int size = *((int*)(ptr)-2);
+ gTotalBytesAlignedAllocs -= size;
+
+ printf("free #%d at address %x, from %s,line %d, size %d\n",gNumAlignedFree,real, filename,line,size);
+
+ sFreeFunc(real);
+ } else
+ {
+ printf("NULL ptr\n");
+ }
+}
+
+#else //BT_DEBUG_MEMORY_ALLOCATIONS
+
+void* btAlignedAllocInternal (size_t size, int alignment)
+{
+ gNumAlignedAllocs++;
+ void* ptr;
+#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
+ ptr = sAlignedAllocFunc(size, alignment);
+#else
+ char *real;
+ unsigned long offset;
+
+ real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
+ if (real) {
+ offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1);
+ ptr = (void *)((real + sizeof(void *)) + offset);
+ *((void **)(ptr)-1) = (void *)(real);
+ } else {
+ ptr = (void *)(real);
+ }
+#endif // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
+// printf("btAlignedAllocInternal %d, %x\n",size,ptr);
+ return ptr;
+}
+
+void btAlignedFreeInternal (void* ptr)
+{
+ if (!ptr)
+ {
+ return;
+ }
+
+ gNumAlignedFree++;
+// printf("btAlignedFreeInternal %x\n",ptr);
+#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
+ sAlignedFreeFunc(ptr);
+#else
+ void* real;
+
+ if (ptr) {
+ real = *((void **)(ptr)-1);
+ sFreeFunc(real);
+ }
+#endif // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
+}
+
+#endif //BT_DEBUG_MEMORY_ALLOCATIONS
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btAxisSweep3.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btAxisSweep3.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btAxisSweep3.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btAxisSweep3.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,37 @@
+
+//Bullet Continuous Collision Detection and Physics Library
+//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+
+//
+// btAxisSweep3
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// 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 "BulletCollision/BroadphaseCollision/btAxisSweep3.h"
+
+
+btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
+:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator)
+{
+ // 1 handle is reserved as sentinel
+ btAssert(maxHandles > 1 && maxHandles < 32767);
+
+}
+
+
+bt32BitAxisSweep3::bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
+:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache,disableRaycastAccelerator)
+{
+ // 1 handle is reserved as sentinel
+ btAssert(maxHandles > 1 && maxHandles < 2147483647);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btBox2dBox2dCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btBox2dBox2dCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btBox2dBox2dCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btBox2dBox2dCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,437 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+* The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.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.
+*/
+
+///btBox2dBox2dCollisionAlgorithm, with modified b2CollidePolygons routines from the Box2D library.
+///The modifications include: switching from b2Vec to btVector3, redefinition of b2Dot, b2Cross
+
+#include "BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
+#include "BulletCollision/CollisionShapes/btBox2dShape.h"
+
+#define USE_PERSISTENT_CONTACTS 1
+
+btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
+: btActivatingCollisionAlgorithm(ci,obj0,obj1),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+ if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
+ {
+ m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
+ m_ownManifold = true;
+ }
+}
+
+btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
+{
+
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr)
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ }
+
+}
+
+
+void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
+
+//#include <stdio.h>
+void btBox2dBox2dCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ if (!m_manifoldPtr)
+ return;
+
+ btCollisionObject* col0 = body0;
+ btCollisionObject* col1 = body1;
+ btBox2dShape* box0 = (btBox2dShape*)col0->getCollisionShape();
+ btBox2dShape* box1 = (btBox2dShape*)col1->getCollisionShape();
+
+ resultOut->setPersistentManifold(m_manifoldPtr);
+
+ b2CollidePolygons(resultOut,box0,col0->getWorldTransform(),box1,col1->getWorldTransform());
+
+ // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
+ if (m_ownManifold)
+ {
+ resultOut->refreshContactPoints();
+ }
+
+}
+
+btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+{
+ //not yet
+ return 1.f;
+}
+
+
+struct ClipVertex
+{
+ btVector3 v;
+ int id;
+ //b2ContactID id;
+ //b2ContactID id;
+};
+
+#define b2Dot(a,b) (a).dot(b)
+#define b2Mul(a,b) (a)*(b)
+#define b2MulT(a,b) (a).transpose()*(b)
+#define b2Cross(a,b) (a).cross(b)
+#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f)
+
+int b2_maxManifoldPoints =2;
+
+static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
+ const btVector3& normal, btScalar offset)
+{
+ // Start with no output points
+ int numOut = 0;
+
+ // Calculate the distance of end points to the line
+ btScalar distance0 = b2Dot(normal, vIn[0].v) - offset;
+ btScalar distance1 = b2Dot(normal, vIn[1].v) - offset;
+
+ // If the points are behind the plane
+ if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
+ if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
+
+ // If the points are on different sides of the plane
+ if (distance0 * distance1 < 0.0f)
+ {
+ // Find intersection point of edge and plane
+ btScalar interp = distance0 / (distance0 - distance1);
+ vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
+ if (distance0 > 0.0f)
+ {
+ vOut[numOut].id = vIn[0].id;
+ }
+ else
+ {
+ vOut[numOut].id = vIn[1].id;
+ }
+ ++numOut;
+ }
+
+ return numOut;
+}
+
+// Find the separation between poly1 and poly2 for a give edge normal on poly1.
+static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1,
+ const btBox2dShape* poly2, const btTransform& xf2)
+{
+ int count1 = poly1->getVertexCount();
+ const btVector3* vertices1 = poly1->getVertices();
+ const btVector3* normals1 = poly1->getNormals();
+
+ int count2 = poly2->getVertexCount();
+ const btVector3* vertices2 = poly2->getVertices();
+
+ btAssert(0 <= edge1 && edge1 < count1);
+
+ // Convert normal from poly1's frame into poly2's frame.
+ btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]);
+ btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World);
+
+ // Find support vertex on poly2 for -normal.
+ int index = 0;
+ btScalar minDot = BT_LARGE_FLOAT;
+
+ for (int i = 0; i < count2; ++i)
+ {
+ btScalar dot = b2Dot(vertices2[i], normal1);
+ if (dot < minDot)
+ {
+ minDot = dot;
+ index = i;
+ }
+ }
+
+ btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
+ btVector3 v2 = b2Mul(xf2, vertices2[index]);
+ btScalar separation = b2Dot(v2 - v1, normal1World);
+ return separation;
+}
+
+// Find the max separation between poly1 and poly2 using edge normals from poly1.
+static btScalar FindMaxSeparation(int* edgeIndex,
+ const btBox2dShape* poly1, const btTransform& xf1,
+ const btBox2dShape* poly2, const btTransform& xf2)
+{
+ int count1 = poly1->getVertexCount();
+ const btVector3* normals1 = poly1->getNormals();
+
+ // Vector pointing from the centroid of poly1 to the centroid of poly2.
+ btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid());
+ btVector3 dLocal1 = b2MulT(xf1.getBasis(), d);
+
+ // Find edge normal on poly1 that has the largest projection onto d.
+ int edge = 0;
+ btScalar maxDot = -BT_LARGE_FLOAT;
+ for (int i = 0; i < count1; ++i)
+ {
+ btScalar dot = b2Dot(normals1[i], dLocal1);
+ if (dot > maxDot)
+ {
+ maxDot = dot;
+ edge = i;
+ }
+ }
+
+ // Get the separation for the edge normal.
+ btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+ if (s > 0.0f)
+ {
+ return s;
+ }
+
+ // Check the separation for the previous edge normal.
+ int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
+ btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
+ if (sPrev > 0.0f)
+ {
+ return sPrev;
+ }
+
+ // Check the separation for the next edge normal.
+ int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
+ btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
+ if (sNext > 0.0f)
+ {
+ return sNext;
+ }
+
+ // Find the best edge and the search direction.
+ int bestEdge;
+ btScalar bestSeparation;
+ int increment;
+ if (sPrev > s && sPrev > sNext)
+ {
+ increment = -1;
+ bestEdge = prevEdge;
+ bestSeparation = sPrev;
+ }
+ else if (sNext > s)
+ {
+ increment = 1;
+ bestEdge = nextEdge;
+ bestSeparation = sNext;
+ }
+ else
+ {
+ *edgeIndex = edge;
+ return s;
+ }
+
+ // Perform a local search for the best edge normal.
+ for ( ; ; )
+ {
+ if (increment == -1)
+ edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
+ else
+ edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
+
+ s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+ if (s > 0.0f)
+ {
+ return s;
+ }
+
+ if (s > bestSeparation)
+ {
+ bestEdge = edge;
+ bestSeparation = s;
+ }
+ else
+ {
+ break;
+ }
+ }
+
+ *edgeIndex = bestEdge;
+ return bestSeparation;
+}
+
+static void FindIncidentEdge(ClipVertex c[2],
+ const btBox2dShape* poly1, const btTransform& xf1, int edge1,
+ const btBox2dShape* poly2, const btTransform& xf2)
+{
+ int count1 = poly1->getVertexCount();
+ const btVector3* normals1 = poly1->getNormals();
+
+ int count2 = poly2->getVertexCount();
+ const btVector3* vertices2 = poly2->getVertices();
+ const btVector3* normals2 = poly2->getNormals();
+
+ btAssert(0 <= edge1 && edge1 < count1);
+
+ // Get the normal of the reference edge in poly2's frame.
+ btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1]));
+
+ // Find the incident edge on poly2.
+ int index = 0;
+ btScalar minDot = BT_LARGE_FLOAT;
+ for (int i = 0; i < count2; ++i)
+ {
+ btScalar dot = b2Dot(normal1, normals2[i]);
+ if (dot < minDot)
+ {
+ minDot = dot;
+ index = i;
+ }
+ }
+
+ // Build the clip vertices for the incident edge.
+ int i1 = index;
+ int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
+
+ c[0].v = b2Mul(xf2, vertices2[i1]);
+// c[0].id.features.referenceEdge = (unsigned char)edge1;
+// c[0].id.features.incidentEdge = (unsigned char)i1;
+// c[0].id.features.incidentVertex = 0;
+
+ c[1].v = b2Mul(xf2, vertices2[i2]);
+// c[1].id.features.referenceEdge = (unsigned char)edge1;
+// c[1].id.features.incidentEdge = (unsigned char)i2;
+// c[1].id.features.incidentVertex = 1;
+}
+
+// Find edge normal of max separation on A - return if separating axis is found
+// Find edge normal of max separation on B - return if separation axis is found
+// Choose reference edge as min(minA, minB)
+// Find incident edge
+// Clip
+
+// The normal points from 1 to 2
+void b2CollidePolygons(btManifoldResult* manifold,
+ const btBox2dShape* polyA, const btTransform& xfA,
+ const btBox2dShape* polyB, const btTransform& xfB)
+{
+
+ int edgeA = 0;
+ btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
+ if (separationA > 0.0f)
+ return;
+
+ int edgeB = 0;
+ btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
+ if (separationB > 0.0f)
+ return;
+
+ const btBox2dShape* poly1; // reference poly
+ const btBox2dShape* poly2; // incident poly
+ btTransform xf1, xf2;
+ int edge1; // reference edge
+ unsigned char flip;
+ const btScalar k_relativeTol = 0.98f;
+ const btScalar k_absoluteTol = 0.001f;
+
+ // TODO_ERIN use "radius" of poly for absolute tolerance.
+ if (separationB > k_relativeTol * separationA + k_absoluteTol)
+ {
+ poly1 = polyB;
+ poly2 = polyA;
+ xf1 = xfB;
+ xf2 = xfA;
+ edge1 = edgeB;
+ flip = 1;
+ }
+ else
+ {
+ poly1 = polyA;
+ poly2 = polyB;
+ xf1 = xfA;
+ xf2 = xfB;
+ edge1 = edgeA;
+ flip = 0;
+ }
+
+ ClipVertex incidentEdge[2];
+ FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
+
+ int count1 = poly1->getVertexCount();
+ const btVector3* vertices1 = poly1->getVertices();
+
+ btVector3 v11 = vertices1[edge1];
+ btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
+
+ btVector3 dv = v12 - v11;
+ btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11);
+ sideNormal.normalize();
+ btVector3 frontNormal = btCrossS(sideNormal, 1.0f);
+
+
+ v11 = b2Mul(xf1, v11);
+ v12 = b2Mul(xf1, v12);
+
+ btScalar frontOffset = b2Dot(frontNormal, v11);
+ btScalar sideOffset1 = -b2Dot(sideNormal, v11);
+ btScalar sideOffset2 = b2Dot(sideNormal, v12);
+
+ // Clip incident edge against extruded edge1 side edges.
+ ClipVertex clipPoints1[2];
+ clipPoints1[0].v.setValue(0,0,0);
+ clipPoints1[1].v.setValue(0,0,0);
+
+ ClipVertex clipPoints2[2];
+ clipPoints2[0].v.setValue(0,0,0);
+ clipPoints2[1].v.setValue(0,0,0);
+
+
+ int np;
+
+ // Clip to box side 1
+ np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
+
+ if (np < 2)
+ return;
+
+ // Clip to negative box side 1
+ np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
+
+ if (np < 2)
+ {
+ return;
+ }
+
+ // Now clipPoints2 contains the clipped points.
+ btVector3 manifoldNormal = flip ? -frontNormal : frontNormal;
+
+ int pointCount = 0;
+ for (int i = 0; i < b2_maxManifoldPoints; ++i)
+ {
+ btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
+
+ if (separation <= 0.0f)
+ {
+
+ //b2ManifoldPoint* cp = manifold->points + pointCount;
+ //btScalar separation = separation;
+ //cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
+ //cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
+
+ manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
+
+// cp->id = clipPoints2[i].id;
+// cp->id.features.flip = flip;
+ ++pointCount;
+ }
+ }
+
+// manifold->pointCount = pointCount;}
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btBox2dShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btBox2dShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btBox2dShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btBox2dShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,42 @@
+/*
+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 "BulletCollision/CollisionShapes/btBox2dShape.h"
+
+
+//{
+
+
+void btBox2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
+}
+
+
+void btBox2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ //btScalar margin = btScalar(0.);
+ btVector3 halfExtents = getHalfExtentsWithMargin();
+
+ btScalar lx=btScalar(2.)*(halfExtents.x());
+ btScalar ly=btScalar(2.)*(halfExtents.y());
+ btScalar lz=btScalar(2.)*(halfExtents.z());
+
+ inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
+ mass/(btScalar(12.0)) * (lx*lx + lz*lz),
+ mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxBoxCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxBoxCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxBoxCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxBoxCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,85 @@
+/*
+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 "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
+
+#define USE_PERSISTENT_CONTACTS 1
+
+btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
+: btActivatingCollisionAlgorithm(ci,obj0,obj1),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+ if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
+ {
+ m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
+ m_ownManifold = true;
+ }
+}
+
+btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm()
+{
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr)
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ }
+}
+
+void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ if (!m_manifoldPtr)
+ return;
+
+ btCollisionObject* col0 = body0;
+ btCollisionObject* col1 = body1;
+ btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape();
+ btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape();
+
+
+
+ /// report a contact. internally this will be kept persistent, and contact reduction is done
+ resultOut->setPersistentManifold(m_manifoldPtr);
+#ifndef USE_PERSISTENT_CONTACTS
+ m_manifoldPtr->clearManifold();
+#endif //USE_PERSISTENT_CONTACTS
+
+ btDiscreteCollisionDetectorInterface::ClosestPointInput input;
+ input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
+ input.m_transformA = body0->getWorldTransform();
+ input.m_transformB = body1->getWorldTransform();
+
+ btBoxBoxDetector detector(box0,box1);
+ detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+#ifdef USE_PERSISTENT_CONTACTS
+ // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
+ if (m_ownManifold)
+ {
+ resultOut->refreshContactPoints();
+ }
+#endif //USE_PERSISTENT_CONTACTS
+
+}
+
+btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+{
+ //not yet
+ return 1.f;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxBoxDetector.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxBoxDetector.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxBoxDetector.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxBoxDetector.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,689 @@
+
+/*
+ * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
+ * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
+ * All rights reserved. Email: russ at q12.org Web: www.q12.org
+ Bullet Continuous Collision Detection and Physics Library
+ Bullet is 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.
+*/
+
+///ODE box-box collision detection is adapted to work with Bullet
+
+#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+
+#include <float.h>
+#include <string.h>
+
+btBoxBoxDetector::btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2)
+: m_box1(box1),
+m_box2(box2)
+{
+
+}
+
+
+// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
+// generate contact points. this returns 0 if there is no contact otherwise
+// it returns the number of contacts generated.
+// `normal' returns the contact normal.
+// `depth' returns the maximum penetration depth along that normal.
+// `return_code' returns a number indicating the type of contact that was
+// detected:
+// 1,2,3 = box 2 intersects with a face of box 1
+// 4,5,6 = box 1 intersects with a face of box 2
+// 7..15 = edge-edge contact
+// `maxc' is the maximum number of contacts allowed to be generated, i.e.
+// the size of the `contact' array.
+// `contact' and `skip' are the contact array information provided to the
+// collision functions. this function only fills in the position and depth
+// fields.
+struct dContactGeom;
+#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
+#define dInfinity FLT_MAX
+
+
+/*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
+PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
+PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
+PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
+*/
+static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
+static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
+static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
+static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); }
+#define dMULTIPLYOP1_331(A,op,B,C) \
+{\
+ (A)[0] op dDOT41((B),(C)); \
+ (A)[1] op dDOT41((B+1),(C)); \
+ (A)[2] op dDOT41((B+2),(C)); \
+}
+
+#define dMULTIPLYOP0_331(A,op,B,C) \
+{ \
+ (A)[0] op dDOT((B),(C)); \
+ (A)[1] op dDOT((B+4),(C)); \
+ (A)[2] op dDOT((B+8),(C)); \
+}
+
+#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
+#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
+
+typedef btScalar dMatrix3[4*3];
+
+void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
+ const btVector3& pb, const btVector3& ub,
+ btScalar *alpha, btScalar *beta);
+void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
+ const btVector3& pb, const btVector3& ub,
+ btScalar *alpha, btScalar *beta)
+{
+ btVector3 p;
+ p[0] = pb[0] - pa[0];
+ p[1] = pb[1] - pa[1];
+ p[2] = pb[2] - pa[2];
+ btScalar uaub = dDOT(ua,ub);
+ btScalar q1 = dDOT(ua,p);
+ btScalar q2 = -dDOT(ub,p);
+ btScalar d = 1-uaub*uaub;
+ if (d <= btScalar(0.0001f)) {
+ // @@@ this needs to be made more robust
+ *alpha = 0;
+ *beta = 0;
+ }
+ else {
+ d = 1.f/d;
+ *alpha = (q1 + uaub*q2)*d;
+ *beta = (uaub*q1 + q2)*d;
+ }
+}
+
+
+
+// find all the intersection points between the 2D rectangle with vertices
+// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
+// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
+//
+// the intersection points are returned as x,y pairs in the 'ret' array.
+// the number of intersection points is returned by the function (this will
+// be in the range 0 to 8).
+
+static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
+{
+ // q (and r) contain nq (and nr) coordinate points for the current (and
+ // chopped) polygons
+ int nq=4,nr=0;
+ btScalar buffer[16];
+ btScalar *q = p;
+ btScalar *r = ret;
+ for (int dir=0; dir <= 1; dir++) {
+ // direction notation: xy[0] = x axis, xy[1] = y axis
+ for (int sign=-1; sign <= 1; sign += 2) {
+ // chop q along the line xy[dir] = sign*h[dir]
+ btScalar *pq = q;
+ btScalar *pr = r;
+ nr = 0;
+ for (int i=nq; i > 0; i--) {
+ // go through all points in q and all lines between adjacent points
+ if (sign*pq[dir] < h[dir]) {
+ // this point is inside the chopping line
+ pr[0] = pq[0];
+ pr[1] = pq[1];
+ pr += 2;
+ nr++;
+ if (nr & 8) {
+ q = r;
+ goto done;
+ }
+ }
+ btScalar *nextq = (i > 1) ? pq+2 : q;
+ if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
+ // this line crosses the chopping line
+ pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
+ (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
+ pr[dir] = sign*h[dir];
+ pr += 2;
+ nr++;
+ if (nr & 8) {
+ q = r;
+ goto done;
+ }
+ }
+ pq += 2;
+ }
+ q = r;
+ r = (q==ret) ? buffer : ret;
+ nq = nr;
+ }
+ }
+ done:
+ if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar));
+ return nr;
+}
+
+
+#define M__PI 3.14159265f
+
+// given n points in the plane (array p, of size 2*n), generate m points that
+// best represent the whole set. the definition of 'best' here is not
+// predetermined - the idea is to select points that give good box-box
+// collision detection behavior. the chosen point indexes are returned in the
+// array iret (of size m). 'i0' is always the first entry in the array.
+// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
+// in the range [0..n-1].
+
+void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]);
+void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
+{
+ // compute the centroid of the polygon in cx,cy
+ int i,j;
+ btScalar a,cx,cy,q;
+ if (n==1) {
+ cx = p[0];
+ cy = p[1];
+ }
+ else if (n==2) {
+ cx = btScalar(0.5)*(p[0] + p[2]);
+ cy = btScalar(0.5)*(p[1] + p[3]);
+ }
+ else {
+ a = 0;
+ cx = 0;
+ cy = 0;
+ for (i=0; i<(n-1); i++) {
+ q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
+ a += q;
+ cx += q*(p[i*2]+p[i*2+2]);
+ cy += q*(p[i*2+1]+p[i*2+3]);
+ }
+ q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
+ if (btFabs(a+q) > SIMD_EPSILON)
+ {
+ a = 1.f/(btScalar(3.0)*(a+q));
+ } else
+ {
+ a=BT_LARGE_FLOAT;
+ }
+ cx = a*(cx + q*(p[n*2-2]+p[0]));
+ cy = a*(cy + q*(p[n*2-1]+p[1]));
+ }
+
+ // compute the angle of each point w.r.t. the centroid
+ btScalar A[8];
+ for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);
+
+ // search for points that have angles closest to A[i0] + i*(2*pi/m).
+ int avail[8];
+ for (i=0; i<n; i++) avail[i] = 1;
+ avail[i0] = 0;
+ iret[0] = i0;
+ iret++;
+ for (j=1; j<m; j++) {
+ a = btScalar(j)*(2*M__PI/m) + A[i0];
+ if (a > M__PI) a -= 2*M__PI;
+ btScalar maxdiff=1e9,diff;
+
+ *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
+
+ for (i=0; i<n; i++) {
+ if (avail[i]) {
+ diff = btFabs (A[i]-a);
+ if (diff > M__PI) diff = 2*M__PI - diff;
+ if (diff < maxdiff) {
+ maxdiff = diff;
+ *iret = i;
+ }
+ }
+ }
+#if defined(DEBUG) || defined (_DEBUG)
+ btAssert (*iret != i0); // ensure iret got set
+#endif
+ avail[*iret] = 0;
+ iret++;
+ }
+}
+
+
+
+int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
+ const btVector3& side1, const btVector3& p2,
+ const dMatrix3 R2, const btVector3& side2,
+ btVector3& normal, btScalar *depth, int *return_code,
+ int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output);
+int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
+ const btVector3& side1, const btVector3& p2,
+ const dMatrix3 R2, const btVector3& side2,
+ btVector3& normal, btScalar *depth, int *return_code,
+ int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output)
+{
+ const btScalar fudge_factor = btScalar(1.05);
+ btVector3 p,pp,normalC(0.f,0.f,0.f);
+ const btScalar *normalR = 0;
+ btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
+ Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
+ int i,j,invert_normal,code;
+
+ // get vector from centers of box 1 to box 2, relative to box 1
+ p = p2 - p1;
+ dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
+
+ // get side lengths / 2
+ A[0] = side1[0]*btScalar(0.5);
+ A[1] = side1[1]*btScalar(0.5);
+ A[2] = side1[2]*btScalar(0.5);
+ B[0] = side2[0]*btScalar(0.5);
+ B[1] = side2[1]*btScalar(0.5);
+ B[2] = side2[2]*btScalar(0.5);
+
+ // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
+ R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
+ R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
+ R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
+
+ Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13);
+ Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23);
+ Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33);
+
+ // for all 15 possible separating axes:
+ // * see if the axis separates the boxes. if so, return 0.
+ // * find the depth of the penetration along the separating axis (s2)
+ // * if this is the largest depth so far, record it.
+ // the normal vector will be set to the separating axis with the smallest
+ // depth. note: normalR is set to point to a column of R1 or R2 if that is
+ // the smallest depth normal so far. otherwise normalR is 0 and normalC is
+ // set to a vector relative to body 1. invert_normal is 1 if the sign of
+ // the normal should be flipped.
+
+#define TST(expr1,expr2,norm,cc) \
+ s2 = btFabs(expr1) - (expr2); \
+ if (s2 > 0) return 0; \
+ if (s2 > s) { \
+ s = s2; \
+ normalR = norm; \
+ invert_normal = ((expr1) < 0); \
+ code = (cc); \
+ }
+
+ s = -dInfinity;
+ invert_normal = 0;
+ code = 0;
+
+ // separating axis = u1,u2,u3
+ TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
+ TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
+ TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
+
+ // separating axis = v1,v2,v3
+ TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
+ TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
+ TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
+
+ // note: cross product axes need to be scaled when s is computed.
+ // normal (n1,n2,n3) is relative to box 1.
+#undef TST
+#define TST(expr1,expr2,n1,n2,n3,cc) \
+ s2 = btFabs(expr1) - (expr2); \
+ if (s2 > 0) return 0; \
+ l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
+ if (l > 0) { \
+ s2 /= l; \
+ if (s2*fudge_factor > s) { \
+ s = s2; \
+ normalR = 0; \
+ normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
+ invert_normal = ((expr1) < 0); \
+ code = (cc); \
+ } \
+ }
+
+ // separating axis = u1 x (v1,v2,v3)
+ TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
+ TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
+ TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
+
+ // separating axis = u2 x (v1,v2,v3)
+ TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
+ TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
+ TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
+
+ // separating axis = u3 x (v1,v2,v3)
+ TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
+ TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
+ TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
+
+#undef TST
+
+ if (!code) return 0;
+
+ // if we get to this point, the boxes interpenetrate. compute the normal
+ // in global coordinates.
+ if (normalR) {
+ normal[0] = normalR[0];
+ normal[1] = normalR[4];
+ normal[2] = normalR[8];
+ }
+ else {
+ dMULTIPLY0_331 (normal,R1,normalC);
+ }
+ if (invert_normal) {
+ normal[0] = -normal[0];
+ normal[1] = -normal[1];
+ normal[2] = -normal[2];
+ }
+ *depth = -s;
+
+ // compute contact point(s)
+
+ if (code > 6) {
+ // an edge from box 1 touches an edge from box 2.
+ // find a point pa on the intersecting edge of box 1
+ btVector3 pa;
+ btScalar sign;
+ for (i=0; i<3; i++) pa[i] = p1[i];
+ for (j=0; j<3; j++) {
+ sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
+ for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
+ }
+
+ // find a point pb on the intersecting edge of box 2
+ btVector3 pb;
+ for (i=0; i<3; i++) pb[i] = p2[i];
+ for (j=0; j<3; j++) {
+ sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
+ for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
+ }
+
+ btScalar alpha,beta;
+ btVector3 ua,ub;
+ for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
+ for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
+
+ dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
+ for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
+ for (i=0; i<3; i++) pb[i] += ub[i]*beta;
+
+ {
+
+ //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
+ //contact[0].depth = *depth;
+ btVector3 pointInWorld;
+
+#ifdef USE_CENTER_POINT
+ for (i=0; i<3; i++)
+ pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
+ output.addContactPoint(-normal,pointInWorld,-*depth);
+#else
+ output.addContactPoint(-normal,pb,-*depth);
+#endif //
+ *return_code = code;
+ }
+ return 1;
+ }
+
+ // okay, we have a face-something intersection (because the separating
+ // axis is perpendicular to a face). define face 'a' to be the reference
+ // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
+ // the incident face (the closest face of the other box).
+
+ const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
+ if (code <= 3) {
+ Ra = R1;
+ Rb = R2;
+ pa = p1;
+ pb = p2;
+ Sa = A;
+ Sb = B;
+ }
+ else {
+ Ra = R2;
+ Rb = R1;
+ pa = p2;
+ pb = p1;
+ Sa = B;
+ Sb = A;
+ }
+
+ // nr = normal vector of reference face dotted with axes of incident box.
+ // anr = absolute values of nr.
+ btVector3 normal2,nr,anr;
+ if (code <= 3) {
+ normal2[0] = normal[0];
+ normal2[1] = normal[1];
+ normal2[2] = normal[2];
+ }
+ else {
+ normal2[0] = -normal[0];
+ normal2[1] = -normal[1];
+ normal2[2] = -normal[2];
+ }
+ dMULTIPLY1_331 (nr,Rb,normal2);
+ anr[0] = btFabs (nr[0]);
+ anr[1] = btFabs (nr[1]);
+ anr[2] = btFabs (nr[2]);
+
+ // find the largest compontent of anr: this corresponds to the normal
+ // for the indident face. the other axis numbers of the indicent face
+ // are stored in a1,a2.
+ int lanr,a1,a2;
+ if (anr[1] > anr[0]) {
+ if (anr[1] > anr[2]) {
+ a1 = 0;
+ lanr = 1;
+ a2 = 2;
+ }
+ else {
+ a1 = 0;
+ a2 = 1;
+ lanr = 2;
+ }
+ }
+ else {
+ if (anr[0] > anr[2]) {
+ lanr = 0;
+ a1 = 1;
+ a2 = 2;
+ }
+ else {
+ a1 = 0;
+ a2 = 1;
+ lanr = 2;
+ }
+ }
+
+ // compute center point of incident face, in reference-face coordinates
+ btVector3 center;
+ if (nr[lanr] < 0) {
+ for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
+ }
+ else {
+ for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
+ }
+
+ // find the normal and non-normal axis numbers of the reference box
+ int codeN,code1,code2;
+ if (code <= 3) codeN = code-1; else codeN = code-4;
+ if (codeN==0) {
+ code1 = 1;
+ code2 = 2;
+ }
+ else if (codeN==1) {
+ code1 = 0;
+ code2 = 2;
+ }
+ else {
+ code1 = 0;
+ code2 = 1;
+ }
+
+ // find the four corners of the incident face, in reference-face coordinates
+ btScalar quad[8]; // 2D coordinate of incident face (x,y pairs)
+ btScalar c1,c2,m11,m12,m21,m22;
+ c1 = dDOT14 (center,Ra+code1);
+ c2 = dDOT14 (center,Ra+code2);
+ // optimize this? - we have already computed this data above, but it is not
+ // stored in an easy-to-index format. for now it's quicker just to recompute
+ // the four dot products.
+ m11 = dDOT44 (Ra+code1,Rb+a1);
+ m12 = dDOT44 (Ra+code1,Rb+a2);
+ m21 = dDOT44 (Ra+code2,Rb+a1);
+ m22 = dDOT44 (Ra+code2,Rb+a2);
+ {
+ btScalar k1 = m11*Sb[a1];
+ btScalar k2 = m21*Sb[a1];
+ btScalar k3 = m12*Sb[a2];
+ btScalar k4 = m22*Sb[a2];
+ quad[0] = c1 - k1 - k3;
+ quad[1] = c2 - k2 - k4;
+ quad[2] = c1 - k1 + k3;
+ quad[3] = c2 - k2 + k4;
+ quad[4] = c1 + k1 + k3;
+ quad[5] = c2 + k2 + k4;
+ quad[6] = c1 + k1 - k3;
+ quad[7] = c2 + k2 - k4;
+ }
+
+ // find the size of the reference face
+ btScalar rect[2];
+ rect[0] = Sa[code1];
+ rect[1] = Sa[code2];
+
+ // intersect the incident and reference faces
+ btScalar ret[16];
+ int n = intersectRectQuad2 (rect,quad,ret);
+ if (n < 1) return 0; // this should never happen
+
+ // convert the intersection points into reference-face coordinates,
+ // and compute the contact position and depth for each point. only keep
+ // those points that have a positive (penetrating) depth. delete points in
+ // the 'ret' array as necessary so that 'point' and 'ret' correspond.
+ btScalar point[3*8]; // penetrating contact points
+ btScalar dep[8]; // depths for those points
+ btScalar det1 = 1.f/(m11*m22 - m12*m21);
+ m11 *= det1;
+ m12 *= det1;
+ m21 *= det1;
+ m22 *= det1;
+ int cnum = 0; // number of penetrating contact points found
+ for (j=0; j < n; j++) {
+ btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
+ btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
+ for (i=0; i<3; i++) point[cnum*3+i] =
+ center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
+ dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
+ if (dep[cnum] >= 0) {
+ ret[cnum*2] = ret[j*2];
+ ret[cnum*2+1] = ret[j*2+1];
+ cnum++;
+ }
+ }
+ if (cnum < 1) return 0; // this should never happen
+
+ // we can't generate more contacts than we actually have
+ if (maxc > cnum) maxc = cnum;
+ if (maxc < 1) maxc = 1;
+
+ if (cnum <= maxc) {
+ // we have less contacts than we need, so we use them all
+ for (j=0; j < cnum; j++) {
+
+ //AddContactPoint...
+
+ //dContactGeom *con = CONTACT(contact,skip*j);
+ //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
+ //con->depth = dep[j];
+
+ btVector3 pointInWorld;
+ for (i=0; i<3; i++)
+ pointInWorld[i] = point[j*3+i] + pa[i];
+ output.addContactPoint(-normal,pointInWorld,-dep[j]);
+
+ }
+ }
+ else {
+ // we have more contacts than are wanted, some of them must be culled.
+ // find the deepest point, it is always the first contact.
+ int i1 = 0;
+ btScalar maxdepth = dep[0];
+ for (i=1; i<cnum; i++) {
+ if (dep[i] > maxdepth) {
+ maxdepth = dep[i];
+ i1 = i;
+ }
+ }
+
+ int iret[8];
+ cullPoints2 (cnum,ret,maxc,i1,iret);
+
+ for (j=0; j < maxc; j++) {
+// dContactGeom *con = CONTACT(contact,skip*j);
+ // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
+ // con->depth = dep[iret[j]];
+
+ btVector3 posInWorld;
+ for (i=0; i<3; i++)
+ posInWorld[i] = point[iret[j]*3+i] + pa[i];
+ output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
+ }
+ cnum = maxc;
+ }
+
+ *return_code = code;
+ return cnum;
+}
+
+void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/)
+{
+
+ const btTransform& transformA = input.m_transformA;
+ const btTransform& transformB = input.m_transformB;
+
+ int skip = 0;
+ dContactGeom *contact = 0;
+
+ dMatrix3 R1;
+ dMatrix3 R2;
+
+ for (int j=0;j<3;j++)
+ {
+ R1[0+4*j] = transformA.getBasis()[j].x();
+ R2[0+4*j] = transformB.getBasis()[j].x();
+
+ R1[1+4*j] = transformA.getBasis()[j].y();
+ R2[1+4*j] = transformB.getBasis()[j].y();
+
+
+ R1[2+4*j] = transformA.getBasis()[j].z();
+ R2[2+4*j] = transformB.getBasis()[j].z();
+
+ }
+
+
+
+ btVector3 normal;
+ btScalar depth;
+ int return_code;
+ int maxc = 4;
+
+
+ dBoxBox2 (transformA.getOrigin(),
+ R1,
+ 2.f*m_box1->getHalfExtentsWithMargin(),
+ transformB.getOrigin(),
+ R2,
+ 2.f*m_box2->getHalfExtentsWithMargin(),
+ normal, &depth, &return_code,
+ maxc, contact, skip,
+ output
+ );
+
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btBoxShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,41 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btBoxShape.h"
+
+
+//{
+
+
+void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
+}
+
+
+void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ //btScalar margin = btScalar(0.);
+ btVector3 halfExtents = getHalfExtentsWithMargin();
+
+ btScalar lx=btScalar(2.)*(halfExtents.x());
+ btScalar ly=btScalar(2.)*(halfExtents.y());
+ btScalar lz=btScalar(2.)*(halfExtents.z());
+
+ inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
+ mass/(btScalar(12.0)) * (lx*lx + lz*lz),
+ mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btBroadphaseProxy.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btBroadphaseProxy.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btBroadphaseProxy.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btBroadphaseProxy.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,17 @@
+/*
+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 "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btBvhTriangleMeshShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btBvhTriangleMeshShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btBvhTriangleMeshShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btBvhTriangleMeshShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,374 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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.
+*/
+
+//#define DISABLE_BVH
+
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
+
+///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
+///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
+btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh)
+:btTriangleMeshShape(meshInterface),
+m_bvh(0),
+m_useQuantizedAabbCompression(useQuantizedAabbCompression),
+m_ownsBvh(false)
+{
+ m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
+ //construct bvh from meshInterface
+#ifndef DISABLE_BVH
+
+ btVector3 bvhAabbMin,bvhAabbMax;
+ if(meshInterface->hasPremadeAabb())
+ {
+ meshInterface->getPremadeAabb(&bvhAabbMin, &bvhAabbMax);
+ }
+ else
+ {
+ meshInterface->calculateAabbBruteForce(bvhAabbMin,bvhAabbMax);
+ }
+
+ if (buildBvh)
+ {
+ void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
+ m_bvh = new (mem) btOptimizedBvh();
+ m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax);
+ m_ownsBvh = true;
+ }
+
+#endif //DISABLE_BVH
+
+}
+
+btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,bool buildBvh)
+:btTriangleMeshShape(meshInterface),
+m_bvh(0),
+m_useQuantizedAabbCompression(useQuantizedAabbCompression),
+m_ownsBvh(false)
+{
+ m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
+ //construct bvh from meshInterface
+#ifndef DISABLE_BVH
+
+ if (buildBvh)
+ {
+ void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
+ m_bvh = new (mem) btOptimizedBvh();
+
+ m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax);
+ m_ownsBvh = true;
+ }
+
+#endif //DISABLE_BVH
+
+}
+
+void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax)
+{
+ m_bvh->refitPartial( m_meshInterface,aabbMin,aabbMax );
+
+ m_localAabbMin.setMin(aabbMin);
+ m_localAabbMax.setMax(aabbMax);
+}
+
+
+void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin,const btVector3& aabbMax)
+{
+ m_bvh->refit( m_meshInterface, aabbMin,aabbMax );
+
+ recalcLocalAabb();
+}
+
+btBvhTriangleMeshShape::~btBvhTriangleMeshShape()
+{
+ if (m_ownsBvh)
+ {
+ m_bvh->~btOptimizedBvh();
+ btAlignedFree(m_bvh);
+ }
+}
+
+void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget)
+{
+ struct MyNodeOverlapCallback : public btNodeOverlapCallback
+ {
+ btStridingMeshInterface* m_meshInterface;
+ btTriangleCallback* m_callback;
+
+ MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
+ :m_meshInterface(meshInterface),
+ m_callback(callback)
+ {
+ }
+
+ virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
+ {
+ btVector3 m_triangle[3];
+ const unsigned char *vertexbase;
+ int numverts;
+ PHY_ScalarType type;
+ int stride;
+ const unsigned char *indexbase;
+ int indexstride;
+ int numfaces;
+ PHY_ScalarType indicestype;
+
+ m_meshInterface->getLockedReadOnlyVertexIndexBase(
+ &vertexbase,
+ numverts,
+ type,
+ stride,
+ &indexbase,
+ indexstride,
+ numfaces,
+ indicestype,
+ nodeSubPart);
+
+ unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
+ btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
+
+ const btVector3& meshScaling = m_meshInterface->getScaling();
+ for (int j=2;j>=0;j--)
+ {
+ int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+
+ 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 */
+ m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
+ m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
+ }
+ };
+
+ MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface);
+
+ m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget);
+}
+
+void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax)
+{
+ struct MyNodeOverlapCallback : public btNodeOverlapCallback
+ {
+ btStridingMeshInterface* m_meshInterface;
+ btTriangleCallback* m_callback;
+
+ MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
+ :m_meshInterface(meshInterface),
+ m_callback(callback)
+ {
+ }
+
+ virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
+ {
+ btVector3 m_triangle[3];
+ const unsigned char *vertexbase;
+ int numverts;
+ PHY_ScalarType type;
+ int stride;
+ const unsigned char *indexbase;
+ int indexstride;
+ int numfaces;
+ PHY_ScalarType indicestype;
+
+ m_meshInterface->getLockedReadOnlyVertexIndexBase(
+ &vertexbase,
+ numverts,
+ type,
+ stride,
+ &indexbase,
+ indexstride,
+ numfaces,
+ indicestype,
+ nodeSubPart);
+
+ unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
+ btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
+
+ const btVector3& meshScaling = m_meshInterface->getScaling();
+ for (int j=2;j>=0;j--)
+ {
+ int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+
+ 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 */
+ m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
+ m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
+ }
+ };
+
+ MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface);
+
+ m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax);
+}
+
+//perform bvh tree traversal and report overlapping triangles to 'callback'
+void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+
+#ifdef DISABLE_BVH
+ //brute force traverse all triangles
+ btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax);
+#else
+
+ //first get all the nodes
+
+
+ struct MyNodeOverlapCallback : public btNodeOverlapCallback
+ {
+ btStridingMeshInterface* m_meshInterface;
+ btTriangleCallback* m_callback;
+ btVector3 m_triangle[3];
+
+
+ MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
+ :m_meshInterface(meshInterface),
+ m_callback(callback)
+ {
+ }
+
+ virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
+ {
+ const unsigned char *vertexbase;
+ int numverts;
+ PHY_ScalarType type;
+ int stride;
+ const unsigned char *indexbase;
+ int indexstride;
+ int numfaces;
+ PHY_ScalarType indicestype;
+
+
+ m_meshInterface->getLockedReadOnlyVertexIndexBase(
+ &vertexbase,
+ numverts,
+ type,
+ stride,
+ &indexbase,
+ indexstride,
+ numfaces,
+ indicestype,
+ nodeSubPart);
+
+ unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
+ btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
+
+ const btVector3& meshScaling = m_meshInterface->getScaling();
+ for (int j=2;j>=0;j--)
+ {
+
+ int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+
+
+#ifdef DEBUG_TRIANGLE_MESH
+ printf("%d ,",graphicsindex);
+#endif //DEBUG_TRIANGLE_MESH
+ 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
+ }
+
+ m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
+ m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
+ }
+
+ };
+
+ MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface);
+
+ m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
+
+
+#endif//DISABLE_BVH
+
+
+}
+
+void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+{
+ if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON)
+ {
+ btTriangleMeshShape::setLocalScaling(scaling);
+ if (m_ownsBvh)
+ {
+ m_bvh->~btOptimizedBvh();
+ btAlignedFree(m_bvh);
+ }
+ ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work
+ void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
+ m_bvh = new(mem) btOptimizedBvh();
+ //rebuild the bvh...
+ m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax);
+ m_ownsBvh = true;
+ }
+}
+
+void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling)
+{
+ btAssert(!m_bvh);
+ btAssert(!m_ownsBvh);
+
+ m_bvh = bvh;
+ m_ownsBvh = false;
+ // update the scaling without rebuilding the bvh
+ if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON)
+ {
+ btTriangleMeshShape::setLocalScaling(scaling);
+ }
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btCapsuleShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btCapsuleShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btCapsuleShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btCapsuleShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,171 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btCapsuleShape.h"
+
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+#include "LinearMath/btQuaternion.h"
+
+btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape ()
+{
+ m_shapeType = CAPSULE_SHAPE_PROXYTYPE;
+ m_upAxis = 1;
+ m_implicitShapeDimensions.setValue(radius,0.5f*height,radius);
+}
+
+
+ btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+
+ btVector3 supVec(0,0,0);
+
+ btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
+
+ btVector3 vec = vec0;
+ btScalar lenSqr = vec.length2();
+ if (lenSqr < btScalar(0.0001))
+ {
+ vec.setValue(1,0,0);
+ } else
+ {
+ btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+ vec *= rlen;
+ }
+
+ btVector3 vtx;
+ btScalar newDot;
+
+ btScalar radius = getRadius();
+
+
+ {
+ btVector3 pos(0,0,0);
+ pos[getUpAxis()] = getHalfHeight();
+
+ vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
+ newDot = vec.dot(vtx);
+ if (newDot > maxDot)
+ {
+ maxDot = newDot;
+ supVec = vtx;
+ }
+ }
+ {
+ btVector3 pos(0,0,0);
+ pos[getUpAxis()] = -getHalfHeight();
+
+ vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
+ newDot = vec.dot(vtx);
+ if (newDot > maxDot)
+ {
+ maxDot = newDot;
+ supVec = vtx;
+ }
+ }
+
+ return supVec;
+
+}
+
+ void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+
+
+ btScalar radius = getRadius();
+
+ for (int j=0;j<numVectors;j++)
+ {
+ btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
+ const btVector3& vec = vectors[j];
+
+ btVector3 vtx;
+ btScalar newDot;
+ {
+ btVector3 pos(0,0,0);
+ pos[getUpAxis()] = getHalfHeight();
+ vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
+ newDot = vec.dot(vtx);
+ if (newDot > maxDot)
+ {
+ maxDot = newDot;
+ supportVerticesOut[j] = vtx;
+ }
+ }
+ {
+ btVector3 pos(0,0,0);
+ pos[getUpAxis()] = -getHalfHeight();
+ vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
+ newDot = vec.dot(vtx);
+ if (newDot > maxDot)
+ {
+ maxDot = newDot;
+ supportVerticesOut[j] = vtx;
+ }
+ }
+
+ }
+}
+
+
+void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ //as an approximation, take the inertia of the box that bounds the spheres
+
+ btTransform ident;
+ ident.setIdentity();
+
+
+ btScalar radius = getRadius();
+
+ btVector3 halfExtents(radius,radius,radius);
+ halfExtents[getUpAxis()]+=getHalfHeight();
+
+ btScalar margin = CONVEX_DISTANCE_MARGIN;
+
+ btScalar lx=btScalar(2.)*(halfExtents[0]+margin);
+ btScalar ly=btScalar(2.)*(halfExtents[1]+margin);
+ btScalar lz=btScalar(2.)*(halfExtents[2]+margin);
+ const btScalar x2 = lx*lx;
+ const btScalar y2 = ly*ly;
+ const btScalar z2 = lz*lz;
+ const btScalar scaledmass = mass * btScalar(.08333333);
+
+ inertia[0] = scaledmass * (y2+z2);
+ inertia[1] = scaledmass * (x2+z2);
+ inertia[2] = scaledmass * (x2+y2);
+
+}
+
+btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height)
+{
+ m_upAxis = 0;
+ m_implicitShapeDimensions.setValue(0.5f*height, radius,radius);
+}
+
+
+
+
+
+
+btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height)
+{
+ m_upAxis = 2;
+ m_implicitShapeDimensions.setValue(radius,radius,0.5f*height);
+}
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,23 @@
+/*
+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 "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+
+btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+{
+ m_dispatcher = ci.m_dispatcher1;
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionDispatcher.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionDispatcher.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionDispatcher.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionDispatcher.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,303 @@
+/*
+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 "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btPoolAllocator.h"
+#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
+
+int gNumManifold = 0;
+
+#ifdef BT_DEBUG
+#include <stdio.h>
+#endif
+
+
+btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration):
+ m_count(0),
+ m_useIslands(true),
+ m_staticWarningReported(false),
+ m_collisionConfiguration(collisionConfiguration)
+{
+ int i;
+
+ setNearCallback(defaultNearCallback);
+
+ m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool();
+
+ m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool();
+
+ for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
+ {
+ for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
+ {
+ m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
+ btAssert(m_doubleDispatch[i][j]);
+ }
+ }
+
+
+}
+
+
+void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
+{
+ m_doubleDispatch[proxyType0][proxyType1] = createFunc;
+}
+
+btCollisionDispatcher::~btCollisionDispatcher()
+{
+}
+
+btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1)
+{
+ gNumManifold++;
+
+ //btAssert(gNumManifold < 65535);
+
+
+ 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())
+ {
+ mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
+ } else
+ {
+ mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
+
+ }
+ btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
+ manifold->m_index1a = m_manifoldsPtr.size();
+ m_manifoldsPtr.push_back(manifold);
+
+ return manifold;
+}
+
+void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
+{
+ manifold->clearManifold();
+}
+
+
+void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
+{
+
+ gNumManifold--;
+
+ //printf("releaseManifold: gNumManifold %d\n",gNumManifold);
+ clearManifold(manifold);
+
+ int findIndex = manifold->m_index1a;
+ btAssert(findIndex < m_manifoldsPtr.size());
+ m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
+ m_manifoldsPtr[findIndex]->m_index1a = findIndex;
+ m_manifoldsPtr.pop_back();
+
+ manifold->~btPersistentManifold();
+ if (m_persistentManifoldPoolAllocator->validPtr(manifold))
+ {
+ m_persistentManifoldPoolAllocator->freeMemory(manifold);
+ } else
+ {
+ btAlignedFree(manifold);
+ }
+
+}
+
+
+
+btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
+{
+
+ btCollisionAlgorithmConstructionInfo ci;
+
+ ci.m_dispatcher1 = this;
+ ci.m_manifold = sharedManifold;
+ btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0,body1);
+
+ return algo;
+}
+
+
+
+
+bool btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1)
+{
+ //here you can do filtering
+ bool hasResponse =
+ (body0->hasContactResponse() && body1->hasContactResponse());
+ //no response between two static/kinematic bodies:
+ hasResponse = hasResponse &&
+ ((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject()));
+ return hasResponse;
+}
+
+bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
+{
+ btAssert(body0);
+ btAssert(body1);
+
+ bool needsCollision = true;
+
+#ifdef BT_DEBUG
+ if (!m_staticWarningReported)
+ {
+ //broadphase filtering already deals with this
+ if ((body0->isStaticObject() || body0->isKinematicObject()) &&
+ (body1->isStaticObject() || body1->isKinematicObject()))
+ {
+ m_staticWarningReported = true;
+ printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
+ }
+ }
+#endif //BT_DEBUG
+
+ if ((!body0->isActive()) && (!body1->isActive()))
+ needsCollision = false;
+ else if (!body0->checkCollideWith(body1))
+ needsCollision = false;
+
+ return needsCollision ;
+
+}
+
+
+
+///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
+///this is useful for the collision dispatcher.
+class btCollisionPairCallback : public btOverlapCallback
+{
+ const btDispatcherInfo& m_dispatchInfo;
+ btCollisionDispatcher* m_dispatcher;
+
+public:
+
+ btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher)
+ :m_dispatchInfo(dispatchInfo),
+ m_dispatcher(dispatcher)
+ {
+ }
+
+ /*btCollisionPairCallback& operator=(btCollisionPairCallback& other)
+ {
+ m_dispatchInfo = other.m_dispatchInfo;
+ m_dispatcher = other.m_dispatcher;
+ return *this;
+ }
+ */
+
+
+ virtual ~btCollisionPairCallback() {}
+
+
+ virtual bool processOverlap(btBroadphasePair& pair)
+ {
+ (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
+
+ return false;
+ }
+};
+
+
+
+void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher)
+{
+ //m_blockedForChanges = true;
+
+ btCollisionPairCallback collisionCallback(dispatchInfo,this);
+
+ pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
+
+ //m_blockedForChanges = false;
+
+}
+
+
+
+
+//by default, Bullet will use this near callback
+void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
+{
+ btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+ btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+ if (dispatcher.needsCollision(colObj0,colObj1))
+ {
+ //dispatcher will keep algorithms persistent in the collision pair
+ if (!collisionPair.m_algorithm)
+ {
+ collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1);
+ }
+
+ if (collisionPair.m_algorithm)
+ {
+ btManifoldResult contactPointResult(colObj0,colObj1);
+
+ if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE)
+ {
+ //discrete collision detection query
+ collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult);
+ } else
+ {
+ //continuous collision detection query, time of impact (toi)
+ btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
+ if (dispatchInfo.m_timeOfImpact > toi)
+ dispatchInfo.m_timeOfImpact = toi;
+
+ }
+ }
+ }
+
+}
+
+
+void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
+{
+ if (m_collisionAlgorithmPoolAllocator->getFreeCount())
+ {
+ return m_collisionAlgorithmPoolAllocator->allocate(size);
+ }
+
+ //warn user for overflow?
+ return btAlignedAlloc(static_cast<size_t>(size), 16);
+}
+
+void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
+{
+ if (m_collisionAlgorithmPoolAllocator->validPtr(ptr))
+ {
+ m_collisionAlgorithmPoolAllocator->freeMemory(ptr);
+ } else
+ {
+ btAlignedFree(ptr);
+ }
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionObject.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionObject.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionObject.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionObject.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,68 @@
+/*
+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 "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btCollisionObject::btCollisionObject()
+ : m_anisotropicFriction(1.f,1.f,1.f),
+ m_hasAnisotropicFriction(false),
+ m_contactProcessingThreshold(BT_LARGE_FLOAT),
+ m_broadphaseHandle(0),
+ m_collisionShape(0),
+ m_rootCollisionShape(0),
+ m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
+ m_islandTag1(-1),
+ m_companionId(-1),
+ m_activationState1(1),
+ m_deactivationTime(btScalar(0.)),
+ m_friction(btScalar(0.5)),
+ m_restitution(btScalar(0.)),
+ m_userObjectPointer(0),
+ m_internalType(CO_COLLISION_OBJECT),
+ m_hitFraction(btScalar(1.)),
+ m_ccdSweptSphereRadius(btScalar(0.)),
+ m_ccdMotionThreshold(btScalar(0.)),
+ m_checkCollideWith(false)
+{
+ m_worldTransform.setIdentity();
+}
+
+btCollisionObject::~btCollisionObject()
+{
+}
+
+void btCollisionObject::setActivationState(int newState)
+{
+ if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
+ m_activationState1 = newState;
+}
+
+void btCollisionObject::forceActivationState(int newState)
+{
+ m_activationState1 = newState;
+}
+
+void btCollisionObject::activate(bool forceActivation)
+{
+ if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT)))
+ {
+ setActivationState(ACTIVE_TAG);
+ m_deactivationTime = btScalar(0.);
+ }
+}
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,98 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "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
+ library is actually installed.
+*/
+extern "C"
+{
+void btBulletCollisionProbe ();
+
+void btBulletCollisionProbe () {}
+}
+
+
+
+void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const
+{
+ btTransform tr;
+ tr.setIdentity();
+ btVector3 aabbMin,aabbMax;
+
+ getAabb(tr,aabbMin,aabbMax);
+
+ radius = (aabbMax-aabbMin).length()*btScalar(0.5);
+ 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);
+ disc += (center).length();
+ return disc;
+}
+
+void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const
+{
+ //start with static aabb
+ getAabb(curTrans,temporalAabbMin,temporalAabbMax);
+
+ btScalar temporalAabbMaxx = temporalAabbMax.getX();
+ btScalar temporalAabbMaxy = temporalAabbMax.getY();
+ btScalar temporalAabbMaxz = temporalAabbMax.getZ();
+ btScalar temporalAabbMinx = temporalAabbMin.getX();
+ btScalar temporalAabbMiny = temporalAabbMin.getY();
+ btScalar temporalAabbMinz = temporalAabbMin.getZ();
+
+ // add linear motion
+ btVector3 linMotion = linvel*timeStep;
+ ///@todo: simd would have a vector max/min operation, instead of per-element access
+ if (linMotion.x() > btScalar(0.))
+ temporalAabbMaxx += linMotion.x();
+ else
+ temporalAabbMinx += linMotion.x();
+ if (linMotion.y() > btScalar(0.))
+ temporalAabbMaxy += linMotion.y();
+ else
+ temporalAabbMiny += linMotion.y();
+ if (linMotion.z() > btScalar(0.))
+ temporalAabbMaxz += linMotion.z();
+ else
+ temporalAabbMinz += linMotion.z();
+
+ //add conservative angular motion
+ btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
+ btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
+ temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
+ temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
+
+ temporalAabbMin -= angularMotion3d;
+ temporalAabbMax += angularMotion3d;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionWorld.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionWorld.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionWorld.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btCollisionWorld.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,846 @@
+/*
+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 "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "LinearMath/btAabbUtil2.h"
+#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"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
+
+
+btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
+:m_dispatcher1(dispatcher),
+m_broadphasePairCache(pairCache),
+m_debugDrawer(0),
+m_forceUpdateAllAabbs(true)
+{
+ m_stackAlloc = collisionConfiguration->getStackAllocator();
+ m_dispatchInfo.m_stackAllocator = m_stackAlloc;
+}
+
+
+btCollisionWorld::~btCollisionWorld()
+{
+
+ //clean up remaining objects
+ int i;
+ for (i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* collisionObject= m_collisionObjects[i];
+
+ btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
+ if (bp)
+ {
+ //
+ // only clear the cached algorithms
+ //
+ getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
+ getBroadphase()->destroyProxy(bp,m_dispatcher1);
+ collisionObject->setBroadphaseHandle(0);
+ }
+ }
+
+
+}
+
+
+
+
+
+
+
+
+
+
+void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+{
+
+ btAssert(collisionObject);
+
+ //check that the object isn't already added
+ btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
+
+ m_collisionObjects.push_back(collisionObject);
+
+ //calculate new AABB
+ btTransform trans = collisionObject->getWorldTransform();
+
+ btVector3 minAabb;
+ btVector3 maxAabb;
+ collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
+
+ int type = collisionObject->getCollisionShape()->getShapeType();
+ collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
+ minAabb,
+ maxAabb,
+ type,
+ collisionObject,
+ collisionFilterGroup,
+ collisionFilterMask,
+ m_dispatcher1,0
+ )) ;
+
+
+
+
+
+}
+
+
+
+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 at continuousphysics.com\n");
+ m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
+ m_debugDrawer->reportErrorWarning("Thanks.\n");
+ }
+ }
+}
+
+void btCollisionWorld::updateAabbs()
+{
+ BT_PROFILE("updateAabbs");
+
+ btTransform predictedTrans;
+ for ( int i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+
+ //only update aabb of active objects
+ if (m_forceUpdateAllAabbs || colObj->isActive())
+ {
+ updateSingleAabb(colObj);
+ }
+ }
+}
+
+
+
+void btCollisionWorld::performDiscreteCollisionDetection()
+{
+ BT_PROFILE("performDiscreteCollisionDetection");
+
+ btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
+ updateAabbs();
+
+ {
+ BT_PROFILE("calculateOverlappingPairs");
+ m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
+ }
+
+
+ btDispatcher* dispatcher = getDispatcher();
+ {
+ BT_PROFILE("dispatchAllCollisionPairs");
+ if (dispatcher)
+ dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
+ }
+
+}
+
+
+
+void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+
+
+ //bool removeFromBroadphase = false;
+
+ {
+
+ btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
+ if (bp)
+ {
+ //
+ // only clear the cached algorithms
+ //
+ getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
+ getBroadphase()->destroyProxy(bp,m_dispatcher1);
+ collisionObject->setBroadphaseHandle(0);
+ }
+ }
+
+
+ //swapremove
+ m_collisionObjects.remove(collisionObject);
+
+}
+
+
+
+void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+ btCollisionObject* collisionObject,
+ const btCollisionShape* collisionShape,
+ const btTransform& colObjWorldTransform,
+ RayResultCallback& resultCallback)
+{
+ btSphereShape pointShape(btScalar(0.0));
+ pointShape.setMargin(0.f);
+ const btConvexShape* castShape = &pointShape;
+
+ if (collisionShape->isConvex())
+ {
+// BT_PROFILE("rayTestConvex");
+ btConvexCast::CastResult castResult;
+ castResult.m_fraction = resultCallback.m_closestHitFraction;
+
+ btConvexShape* convexShape = (btConvexShape*) collisionShape;
+ btVoronoiSimplexSolver simplexSolver;
+#define USE_SUBSIMPLEX_CONVEX_CAST 1
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+ btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
+#else
+ //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver);
+ //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
+#endif //#USE_SUBSIMPLEX_CONVEX_CAST
+
+ if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
+ {
+ //add hit
+ if (castResult.m_normal.length2() > btScalar(0.0001))
+ {
+ if (castResult.m_fraction < resultCallback.m_closestHitFraction)
+ {
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+ //rotate normal into worldspace
+ castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
+#endif //USE_SUBSIMPLEX_CONVEX_CAST
+
+ castResult.m_normal.normalize();
+ btCollisionWorld::LocalRayResult localRayResult
+ (
+ collisionObject,
+ 0,
+ castResult.m_normal,
+ castResult.m_fraction
+ );
+
+ bool normalInWorldSpace = true;
+ resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
+
+ }
+ }
+ }
+ } else {
+ if (collisionShape->isConcave())
+ {
+// BT_PROFILE("rayTestConcave");
+ if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+ {
+ ///optimized version for btBvhTriangleMeshShape
+ btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
+ btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+ btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+ btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+ //ConvexCast::CastResult
+ struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
+ {
+ btCollisionWorld::RayResultCallback* m_resultCallback;
+ btCollisionObject* m_collisionObject;
+ btTriangleMeshShape* m_triangleMesh;
+
+ btTransform m_colObjWorldTransform;
+
+ BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
+ btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform):
+ //@BP Mod
+ btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+ m_resultCallback(resultCallback),
+ m_collisionObject(collisionObject),
+ m_triangleMesh(triangleMesh),
+ m_colObjWorldTransform(colObjWorldTransform)
+ {
+ }
+
+
+ virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
+ {
+ btCollisionWorld::LocalShapeInfo shapeInfo;
+ shapeInfo.m_shapePart = partId;
+ shapeInfo.m_triangleIndex = triangleIndex;
+
+ btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
+
+ btCollisionWorld::LocalRayResult rayResult
+ (m_collisionObject,
+ &shapeInfo,
+ hitNormalWorld,
+ hitFraction);
+
+ bool normalInWorldSpace = true;
+ return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
+ }
+
+ };
+
+ BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform);
+ rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+ triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
+ } else
+ {
+ //generic (slower) case
+ btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
+
+ btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+
+ btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+ btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+ //ConvexCast::CastResult
+
+ struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
+ {
+ btCollisionWorld::RayResultCallback* m_resultCallback;
+ btCollisionObject* m_collisionObject;
+ btConcaveShape* m_triangleMesh;
+
+ btTransform m_colObjWorldTransform;
+
+ BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
+ btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
+ //@BP Mod
+ btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+ m_resultCallback(resultCallback),
+ m_collisionObject(collisionObject),
+ m_triangleMesh(triangleMesh),
+ m_colObjWorldTransform(colObjWorldTransform)
+ {
+ }
+
+
+ virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
+ {
+ btCollisionWorld::LocalShapeInfo shapeInfo;
+ shapeInfo.m_shapePart = partId;
+ shapeInfo.m_triangleIndex = triangleIndex;
+
+ btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
+
+ btCollisionWorld::LocalRayResult rayResult
+ (m_collisionObject,
+ &shapeInfo,
+ hitNormalWorld,
+ hitFraction);
+
+ bool normalInWorldSpace = true;
+ return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
+ }
+
+ };
+
+
+ BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
+ rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+
+ btVector3 rayAabbMinLocal = rayFromLocal;
+ rayAabbMinLocal.setMin(rayToLocal);
+ btVector3 rayAabbMaxLocal = rayFromLocal;
+ rayAabbMaxLocal.setMax(rayToLocal);
+
+ concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
+ }
+ } else {
+// BT_PROFILE("rayTestCompound");
+ ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
+ if (collisionShape->isCompound())
+ {
+ const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+ int i=0;
+ for (i=0;i<compoundShape->getNumChildShapes();i++)
+ {
+ btTransform childTrans = compoundShape->getChildTransform(i);
+ const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
+ btTransform childWorldTrans = colObjWorldTransform * childTrans;
+ // replace collision shape so that callback can determine the triangle
+ btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
+ collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
+ rayTestSingle(rayFromTrans,rayToTrans,
+ collisionObject,
+ childCollisionShape,
+ childWorldTrans,
+ resultCallback);
+ // restore
+ collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
+ }
+ }
+ }
+ }
+}
+
+void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
+ btCollisionObject* collisionObject,
+ const btCollisionShape* collisionShape,
+ const btTransform& colObjWorldTransform,
+ ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+{
+ if (collisionShape->isConvex())
+ {
+ //BT_PROFILE("convexSweepConvex");
+ btConvexCast::CastResult castResult;
+ castResult.m_allowedPenetration = allowedPenetration;
+ castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
+
+ btConvexShape* convexShape = (btConvexShape*) collisionShape;
+ btVoronoiSimplexSolver simplexSolver;
+ btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
+
+ btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
+ //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
+ //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
+
+ btConvexCast* castPtr = &convexCaster1;
+
+
+
+ if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
+ {
+ //add hit
+ if (castResult.m_normal.length2() > btScalar(0.0001))
+ {
+ if (castResult.m_fraction < resultCallback.m_closestHitFraction)
+ {
+ castResult.m_normal.normalize();
+ btCollisionWorld::LocalConvexResult localConvexResult
+ (
+ collisionObject,
+ 0,
+ castResult.m_normal,
+ castResult.m_hitPoint,
+ castResult.m_fraction
+ );
+
+ bool normalInWorldSpace = true;
+ resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
+
+ }
+ }
+ }
+ } else {
+ if (collisionShape->isConcave())
+ {
+ if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+ {
+ //BT_PROFILE("convexSweepbtBvhTriangleMesh");
+ btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
+ btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+ btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
+ btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
+ // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
+ btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
+
+ //ConvexCast::CastResult
+ struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
+ {
+ btCollisionWorld::ConvexResultCallback* m_resultCallback;
+ btCollisionObject* m_collisionObject;
+ btTriangleMeshShape* m_triangleMesh;
+
+ BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
+ btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
+ btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
+ m_resultCallback(resultCallback),
+ m_collisionObject(collisionObject),
+ m_triangleMesh(triangleMesh)
+ {
+ }
+
+
+ virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
+ {
+ btCollisionWorld::LocalShapeInfo shapeInfo;
+ shapeInfo.m_shapePart = partId;
+ shapeInfo.m_triangleIndex = triangleIndex;
+ if (hitFraction <= m_resultCallback->m_closestHitFraction)
+ {
+
+ btCollisionWorld::LocalConvexResult convexResult
+ (m_collisionObject,
+ &shapeInfo,
+ hitNormalLocal,
+ hitPointLocal,
+ hitFraction);
+
+ bool normalInWorldSpace = true;
+
+
+ return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
+ }
+ return hitFraction;
+ }
+
+ };
+
+ BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
+ tccb.m_hitFraction = resultCallback.m_closestHitFraction;
+ btVector3 boxMinLocal, boxMaxLocal;
+ castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
+ triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
+ } else
+ {
+ //BT_PROFILE("convexSweepConcave");
+ btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
+ btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+ btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
+ btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
+ // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
+ btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
+
+ //ConvexCast::CastResult
+ struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
+ {
+ btCollisionWorld::ConvexResultCallback* m_resultCallback;
+ btCollisionObject* m_collisionObject;
+ btConcaveShape* m_triangleMesh;
+
+ BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
+ btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
+ btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
+ m_resultCallback(resultCallback),
+ m_collisionObject(collisionObject),
+ m_triangleMesh(triangleMesh)
+ {
+ }
+
+
+ virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
+ {
+ btCollisionWorld::LocalShapeInfo shapeInfo;
+ shapeInfo.m_shapePart = partId;
+ shapeInfo.m_triangleIndex = triangleIndex;
+ if (hitFraction <= m_resultCallback->m_closestHitFraction)
+ {
+
+ btCollisionWorld::LocalConvexResult convexResult
+ (m_collisionObject,
+ &shapeInfo,
+ hitNormalLocal,
+ hitPointLocal,
+ hitFraction);
+
+ bool normalInWorldSpace = false;
+
+ return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
+ }
+ return hitFraction;
+ }
+
+ };
+
+ BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
+ tccb.m_hitFraction = resultCallback.m_closestHitFraction;
+ btVector3 boxMinLocal, boxMaxLocal;
+ castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
+
+ btVector3 rayAabbMinLocal = convexFromLocal;
+ rayAabbMinLocal.setMin(convexToLocal);
+ btVector3 rayAabbMaxLocal = convexFromLocal;
+ rayAabbMaxLocal.setMax(convexToLocal);
+ rayAabbMinLocal += boxMinLocal;
+ rayAabbMaxLocal += boxMaxLocal;
+ concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
+ }
+ } else {
+ ///@todo : use AABB tree or other BVH acceleration structure!
+ if (collisionShape->isCompound())
+ {
+ BT_PROFILE("convexSweepCompound");
+ const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+ int i=0;
+ for (i=0;i<compoundShape->getNumChildShapes();i++)
+ {
+ btTransform childTrans = compoundShape->getChildTransform(i);
+ const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
+ btTransform childWorldTrans = colObjWorldTransform * childTrans;
+ // replace collision shape so that callback can determine the triangle
+ btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
+ collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
+ objectQuerySingle(castShape, convexFromTrans,convexToTrans,
+ collisionObject,
+ childCollisionShape,
+ childWorldTrans,
+ resultCallback, allowedPenetration);
+ // restore
+ collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
+ }
+ }
+ }
+ }
+}
+
+
+struct btSingleRayCallback : public btBroadphaseRayCallback
+{
+
+ btVector3 m_rayFromWorld;
+ btVector3 m_rayToWorld;
+ btTransform m_rayFromTrans;
+ btTransform m_rayToTrans;
+ btVector3 m_hitNormal;
+
+ const btCollisionWorld* m_world;
+ btCollisionWorld::RayResultCallback& m_resultCallback;
+
+ 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);
+
+ btVector3 rayDir = (rayToWorld-rayFromWorld);
+
+ rayDir.normalize ();
+ ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
+ m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+ m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+ m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : 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 (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();
+ //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+#if 0
+#ifdef RECALCULATE_AABB
+ btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+ collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#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))
+ {
+ m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
+ collisionObject,
+ collisionObject->getCollisionShape(),
+ collisionObject->getWorldTransform(),
+ 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;i<this->getNumCollisionObjects();i++)
+ {
+ rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+ }
+#endif //USE_BRUTEFORCE_RAYBROADPHASE
+
+}
+
+
+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/BT_LARGE_FLOAT
+ m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+ m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+ m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : 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;
+ btVector3 castShapeAabbMin, castShapeAabbMax;
+ /* Compute AABB that encompasses angular movement */
+ {
+ 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, 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;
+ for (i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* collisionObject= m_collisionObjects[i];
+ //only perform raycast if filterMask matches
+ if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
+ //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))
+ {
+ objectQuerySingle(castShape, convexFromTrans,convexToTrans,
+ collisionObject,
+ collisionObject->getCollisionShape(),
+ collisionObject->getWorldTransform(),
+ resultCallback,
+ allowedCcdPenetration);
+ }
+ }
+ }
+#endif //USE_BRUTEFORCE_RAYBROADPHASE
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btCompoundCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btCompoundCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btCompoundCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btCompoundCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,351 @@
+/*
+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 "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+: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<btCompoundShape*>(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;
+ btAssert (colObj->getCollisionShape()->isCompound());
+
+ btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
+ int numChildren = compoundShape->getNumChildShapes();
+ int i;
+
+ m_childCollisionAlgorithms.resize(numChildren);
+ for (i=0;i<numChildren;i++)
+ {
+ if (compoundShape->getDynamicAabbTree())
+ {
+ m_childCollisionAlgorithms[i] = 0;
+ } else
+ {
+ btCollisionShape* tmpShape = colObj->getCollisionShape();
+ btCollisionShape* childShape = compoundShape->getChildShape(i);
+ colObj->internalSetTemporaryCollisionShape( childShape );
+ m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(colObj,otherObj,m_sharedManifold);
+ colObj->internalSetTemporaryCollisionShape( tmpShape );
+ }
+ }
+}
+
+void btCompoundCollisionAlgorithm::removeChildAlgorithms()
+{
+ int numChildren = m_childCollisionAlgorithms.size();
+ int i;
+ for (i=0;i<numChildren;i++)
+ {
+ if (m_childCollisionAlgorithms[i])
+ {
+ m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+ m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+ }
+ }
+}
+
+btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
+{
+ removeChildAlgorithms();
+}
+
+
+
+
+struct btCompoundLeafCallback : btDbvt::ICollide
+{
+
+public:
+
+ btCollisionObject* m_compoundColObj;
+ btCollisionObject* m_otherObj;
+ btDispatcher* m_dispatcher;
+ const btDispatcherInfo& m_dispatchInfo;
+ btManifoldResult* m_resultOut;
+ btCollisionAlgorithm** m_childCollisionAlgorithms;
+ btPersistentManifold* m_sharedManifold;
+
+
+
+
+ btCompoundLeafCallback (btCollisionObject* compoundObj,btCollisionObject* otherObj,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut,btCollisionAlgorithm** childCollisionAlgorithms,btPersistentManifold* sharedManifold)
+ :m_compoundColObj(compoundObj),m_otherObj(otherObj),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+ m_childCollisionAlgorithms(childCollisionAlgorithms),
+ m_sharedManifold(sharedManifold)
+ {
+
+ }
+
+
+ void ProcessChildShape(btCollisionShape* childShape,int index)
+ {
+
+ btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+
+
+ //backup
+ btTransform orgTrans = m_compoundColObj->getWorldTransform();
+ btTransform orgInterpolationTrans = m_compoundColObj->getInterpolationWorldTransform();
+ const btTransform& childTrans = compoundShape->getChildTransform(index);
+ btTransform newChildWorldTrans = orgTrans*childTrans ;
+
+ //perform an AABB check first
+ btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+ childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+ m_otherObj->getCollisionShape()->getAabb(m_otherObj->getWorldTransform(),aabbMin1,aabbMax1);
+
+ if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+ {
+
+ m_compoundColObj->setWorldTransform( newChildWorldTrans);
+ m_compoundColObj->setInterpolationWorldTransform(newChildWorldTrans);
+
+ //the contactpoint is still projected back using the original inverted worldtrans
+ btCollisionShape* tmpShape = m_compoundColObj->getCollisionShape();
+ m_compoundColObj->internalSetTemporaryCollisionShape( childShape );
+
+ if (!m_childCollisionAlgorithms[index])
+ m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
+
+ ///detect swapping case
+ if (m_resultOut->getBody0Internal() == m_compoundColObj)
+ {
+ m_resultOut->setShapeIdentifiersA(-1,index);
+ } else
+ {
+ m_resultOut->setShapeIdentifiersB(-1,index);
+ }
+
+ m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut);
+ if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+ {
+ btVector3 worldAabbMin,worldAabbMax;
+ m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
+ m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
+ }
+
+ //revert back transform
+ m_compoundColObj->internalSetTemporaryCollisionShape( tmpShape);
+ m_compoundColObj->setWorldTransform( orgTrans );
+ m_compoundColObj->setInterpolationWorldTransform(orgInterpolationTrans);
+ }
+ }
+ void Process(const btDbvtNode* leaf)
+ {
+ int index = leaf->dataAsInt;
+
+ btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+ btCollisionShape* childShape = compoundShape->getChildShape(index);
+ if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+ {
+ btVector3 worldAabbMin,worldAabbMax;
+ btTransform orgTrans = m_compoundColObj->getWorldTransform();
+ btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
+ m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
+ }
+ ProcessChildShape(childShape,index);
+
+ }
+};
+
+
+
+
+
+
+void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ btCollisionObject* colObj = m_isSwapped? body1 : body0;
+ btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
+
+
+ btAssert (colObj->getCollisionShape()->isCompound());
+ btCompoundShape* compoundShape = static_cast<btCompoundShape*>(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;i<m_childCollisionAlgorithms.size();i++)
+ {
+ if (m_childCollisionAlgorithms[i])
+ {
+ m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+ for (int m=0;m<manifoldArray.size();m++)
+ {
+ if (manifoldArray[m]->getNumContacts())
+ {
+ resultOut->setPersistentManifold(manifoldArray[m]);
+ resultOut->refreshContactPoints();
+ resultOut->setPersistentManifold(0);//??necessary?
+ }
+ }
+ manifoldArray.clear();
+ }
+ }
+ }
+
+ if (tree)
+ {
+
+ btVector3 localAabbMin,localAabbMax;
+ btTransform otherInCompoundSpace;
+ otherInCompoundSpace = colObj->getWorldTransform().inverse() * otherObj->getWorldTransform();
+ otherObj->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
+
+ const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+ //process all children, that overlap with the given AABB bounds
+ tree->collideTV(tree->m_root,bounds,callback);
+
+ } else
+ {
+ //iterate over all children, perform an AABB check inside ProcessChildShape
+ int numChildren = m_childCollisionAlgorithms.size();
+ int i;
+ for (i=0;i<numChildren;i++)
+ {
+ callback.ProcessChildShape(compoundShape->getChildShape(i),i);
+ }
+ }
+
+ {
+ //iterate over all children, perform an AABB check inside ProcessChildShape
+ int numChildren = m_childCollisionAlgorithms.size();
+ int i;
+ btManifoldArray manifoldArray;
+
+ for (i=0;i<numChildren;i++)
+ {
+ if (m_childCollisionAlgorithms[i])
+ {
+ btCollisionShape* childShape = compoundShape->getChildShape(i);
+ //if not longer overlapping, remove the algorithm
+ btTransform orgTrans = colObj->getWorldTransform();
+ btTransform orgInterpolationTrans = colObj->getInterpolationWorldTransform();
+ const btTransform& childTrans = compoundShape->getChildTransform(i);
+ btTransform newChildWorldTrans = orgTrans*childTrans ;
+
+ //perform an AABB check first
+ btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+ childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+ otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1);
+
+ if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+ {
+ m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+ m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+ m_childCollisionAlgorithms[i] = 0;
+ }
+
+ }
+
+ }
+
+
+
+ }
+}
+
+btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+ btCollisionObject* colObj = m_isSwapped? body1 : body0;
+ btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
+ btAssert (colObj->getCollisionShape()->isCompound());
+
+ btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
+ //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
+ //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
+ //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
+ //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
+ //then use each overlapping node AABB against Tree0
+ //and vise versa.
+
+ btScalar hitFraction = btScalar(1.);
+
+ int numChildren = m_childCollisionAlgorithms.size();
+ int i;
+ for (i=0;i<numChildren;i++)
+ {
+ //temporarily exchange parent btCollisionShape with childShape, and recurse
+ btCollisionShape* childShape = compoundShape->getChildShape(i);
+
+ //backup
+ btTransform orgTrans = colObj->getWorldTransform();
+
+ const btTransform& childTrans = compoundShape->getChildTransform(i);
+ //btTransform newChildWorldTrans = orgTrans*childTrans ;
+ colObj->setWorldTransform( orgTrans*childTrans );
+
+ btCollisionShape* tmpShape = colObj->getCollisionShape();
+ colObj->internalSetTemporaryCollisionShape( childShape );
+ btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
+ if (frac<hitFraction)
+ {
+ hitFraction = frac;
+ }
+ //revert back
+ colObj->internalSetTemporaryCollisionShape( tmpShape);
+ colObj->setWorldTransform( orgTrans);
+ }
+ return hitFraction;
+
+}
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btCompoundShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btCompoundShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btCompoundShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btCompoundShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,266 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+
+btCompoundShape::btCompoundShape(bool enableDynamicAabbTree)
+: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)),
+m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)),
+m_dynamicAabbTree(0),
+m_updateRevision(1),
+m_collisionMargin(btScalar(0.)),
+m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
+{
+ m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
+
+ if (enableDynamicAabbTree)
+ {
+ void* mem = btAlignedAlloc(sizeof(btDbvt),16);
+ m_dynamicAabbTree = new(mem) btDbvt();
+ btAssert(mem==m_dynamicAabbTree);
+ }
+}
+
+
+btCompoundShape::~btCompoundShape()
+{
+ if (m_dynamicAabbTree)
+ {
+ m_dynamicAabbTree->~btDbvt();
+ btAlignedFree(m_dynamicAabbTree);
+ }
+}
+
+void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
+{
+ m_updateRevision++;
+ //m_childTransforms.push_back(localTransform);
+ //m_childShapes.push_back(shape);
+ btCompoundShapeChild child;
+ child.m_transform = localTransform;
+ child.m_childShape = shape;
+ child.m_childShapeType = shape->getShapeType();
+ child.m_childMargin = shape->getMargin();
+
+
+ //extend the local aabbMin/aabbMax
+ btVector3 localAabbMin,localAabbMax;
+ shape->getAabb(localTransform,localAabbMin,localAabbMax);
+ for (int i=0;i<3;i++)
+ {
+ if (m_localAabbMin[i] > localAabbMin[i])
+ {
+ m_localAabbMin[i] = localAabbMin[i];
+ }
+ if (m_localAabbMax[i] < localAabbMax[i])
+ {
+ m_localAabbMax[i] = localAabbMax[i];
+ }
+
+ }
+ if (m_dynamicAabbTree)
+ {
+ const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+ int index = m_children.size();
+ child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
+ }
+
+ 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)
+ {
+ m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
+ }
+ m_children.swap(childShapeIndex,m_children.size()-1);
+ m_children.pop_back();
+
+}
+
+
+
+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--)
+ {
+ if(m_children[i].m_childShape == shape)
+ {
+ removeChildShapeByIndex(i);
+ }
+ }
+
+
+
+ recalculateLocalAabb();
+}
+
+void btCompoundShape::recalculateLocalAabb()
+{
+ // Recalculate the local aabb
+ // Brute force, it iterates over all the shapes left.
+
+ m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+
+ //extend the local aabbMin/aabbMax
+ for (int j = 0; j < m_children.size(); j++)
+ {
+ btVector3 localAabbMin,localAabbMax;
+ m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax);
+ for (int i=0;i<3;i++)
+ {
+ if (m_localAabbMin[i] > localAabbMin[i])
+ m_localAabbMin[i] = localAabbMin[i];
+ if (m_localAabbMax[i] < localAabbMax[i])
+ m_localAabbMax[i] = localAabbMax[i];
+ }
+ }
+}
+
+///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
+ 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();
+
+ 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
+{
+ //approximation: take the inertia from the aabb for now
+ btTransform ident;
+ ident.setIdentity();
+ btVector3 aabbMin,aabbMax;
+ getAabb(ident,aabbMin,aabbMax);
+
+ btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
+
+ btScalar lx=btScalar(2.)*(halfExtents.x());
+ btScalar ly=btScalar(2.)*(halfExtents.y());
+ btScalar lz=btScalar(2.)*(halfExtents.z());
+
+ inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
+ inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
+ inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
+
+}
+
+
+
+
+void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
+{
+ int n = m_children.size();
+
+ btScalar totalMass = 0;
+ btVector3 center(0, 0, 0);
+ int k;
+
+ for (k = 0; k < n; k++)
+ {
+ center += m_children[k].m_transform.getOrigin() * masses[k];
+ totalMass += masses[k];
+ }
+ center /= totalMass;
+ principal.setOrigin(center);
+
+ btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
+ for ( k = 0; k < n; k++)
+ {
+ btVector3 i;
+ m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
+
+ const btTransform& t = m_children[k].m_transform;
+ btVector3 o = t.getOrigin() - center;
+
+ //compute inertia tensor in coordinate system of compound shape
+ btMatrix3x3 j = t.getBasis().transpose();
+ j[0] *= i[0];
+ j[1] *= i[1];
+ j[2] *= i[2];
+ j = t.getBasis() * j;
+
+ //add inertia tensor
+ tensor[0] += j[0];
+ tensor[1] += j[1];
+ tensor[2] += j[2];
+
+ //compute inertia tensor of pointmass at o
+ btScalar o2 = o.length2();
+ j[0].setValue(o2, 0, 0);
+ j[1].setValue(0, o2, 0);
+ j[2].setValue(0, 0, o2);
+ j[0] += o * -o.x();
+ j[1] += o * -o.y();
+ j[2] += o * -o.z();
+
+ //add inertia tensor of pointmass
+ tensor[0] += masses[k] * j[0];
+ tensor[1] += masses[k] * j[1];
+ tensor[2] += masses[k] * j[2];
+ }
+
+ tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+ inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
+}
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConcaveShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConcaveShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConcaveShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConcaveShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,27 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btConcaveShape.h"
+
+btConcaveShape::btConcaveShape() : m_collisionMargin(btScalar(0.))
+{
+
+}
+
+btConcaveShape::~btConcaveShape()
+{
+
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConeShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConeShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConeShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConeShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,133 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btConeShape.h"
+
+
+
+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));
+}
+
+btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height):
+btConeShape(radius,height)
+{
+ setConeUpIndex(2);
+}
+
+btConeShapeX::btConeShapeX (btScalar radius,btScalar height):
+btConeShape(radius,height)
+{
+ setConeUpIndex(0);
+}
+
+///choose upAxis index
+void btConeShape::setConeUpIndex(int upIndex)
+{
+ switch (upIndex)
+ {
+ case 0:
+ m_coneIndices[0] = 1;
+ m_coneIndices[1] = 0;
+ m_coneIndices[2] = 2;
+ break;
+ case 1:
+ m_coneIndices[0] = 0;
+ m_coneIndices[1] = 1;
+ m_coneIndices[2] = 2;
+ break;
+ case 2:
+ m_coneIndices[0] = 0;
+ m_coneIndices[1] = 2;
+ m_coneIndices[2] = 1;
+ break;
+ default:
+ btAssert(0);
+ };
+}
+
+btVector3 btConeShape::coneLocalSupport(const btVector3& v) const
+{
+
+ btScalar halfHeight = m_height * btScalar(0.5);
+
+ if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
+ {
+ btVector3 tmp;
+
+ tmp[m_coneIndices[0]] = btScalar(0.);
+ tmp[m_coneIndices[1]] = halfHeight;
+ tmp[m_coneIndices[2]] = btScalar(0.);
+ return tmp;
+ }
+ else {
+ btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
+ if (s > SIMD_EPSILON) {
+ btScalar d = m_radius / s;
+ btVector3 tmp;
+ tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
+ tmp[m_coneIndices[1]] = -halfHeight;
+ tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
+ return tmp;
+ }
+ else {
+ btVector3 tmp;
+ tmp[m_coneIndices[0]] = btScalar(0.);
+ tmp[m_coneIndices[1]] = -halfHeight;
+ tmp[m_coneIndices[2]] = btScalar(0.);
+ return tmp;
+ }
+ }
+
+}
+
+btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
+{
+ return coneLocalSupport(vec);
+}
+
+void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+ for (int i=0;i<numVectors;i++)
+ {
+ const btVector3& vec = vectors[i];
+ supportVerticesOut[i] = coneLocalSupport(vec);
+ }
+}
+
+
+btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const
+{
+ btVector3 supVertex = coneLocalSupport(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;
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConeTwistConstraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConeTwistConstraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConeTwistConstraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConeTwistConstraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1026 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
+
+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.
+
+Written by: Marcus Hennix
+*/
+
+
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btMinMax.h"
+#include <new>
+
+
+
+//#define CONETWIST_USE_OBSOLETE_SOLVER true
+#define CONETWIST_USE_OBSOLETE_SOLVER false
+#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
+
+
+SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
+{
+ btVector3 vec = axis * invInertiaWorld;
+ return axis.dot(vec);
+}
+
+
+btConeTwistConstraint::btConeTwistConstraint()
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+{
+}
+
+
+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_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+{
+ init();
+}
+
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
+ :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
+ m_angularOnly(false),
+ m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+{
+ m_rbBFrame = m_rbAFrame;
+ init();
+}
+
+
+void btConeTwistConstraint::init()
+{
+ m_angularOnly = false;
+ m_solveTwistLimit = false;
+ m_solveSwingLimit = false;
+ m_bMotorEnabled = false;
+ m_maxMotorImpulse = btScalar(-1);
+
+ setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+ m_damping = btScalar(0.01);
+ m_fixThresh = CONETWIST_DEF_FIX_THRESH;
+}
+
+
+void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ }
+ else
+ {
+ info->m_numConstraintRows = 3;
+ info->nub = 3;
+ calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+ 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--;
+ }
+ }
+}
+
+void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+ //always reserve 6 rows: object transform is not available on SPU
+ info->m_numConstraintRows = 6;
+ info->nub = 0;
+
+}
+
+
+void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+{
+ getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+}
+
+void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+{
+ calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
+
+ btAssert(!m_useSolveConstraintObsolete);
+ // set jacobian
+ info->m_J1linearAxis[0] = 1;
+ info->m_J1linearAxis[info->rowskip+1] = 1;
+ info->m_J1linearAxis[2*info->rowskip+2] = 1;
+ btVector3 a1 = transA.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 = transB.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] + transB.getOrigin()[j] - a1[j] - transA.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 = transA*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)
+ {
+
+ 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
+ {
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ srow += info->rowskip;
+ }
+}
+
+
+
+void btConeTwistConstraint::buildJacobian()
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ m_appliedImpulse = btScalar(0.);
+ m_accTwistLimitImpulse = btScalar(0.);
+ m_accSwingLimitImpulse = btScalar(0.);
+ m_accMotorImpulse = btVector3(0.,0.,0.);
+
+ if (!m_angularOnly)
+ {
+ 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(),
+ pivotBInW - m_rbB.getCenterOfMassPosition(),
+ normal[i],
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbA.getInvMass(),
+ m_rbB.getInvInertiaDiagLocal(),
+ m_rbB.getInvMass());
+ }
+ }
+
+ calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+ }
+}
+
+
+
+void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+ #ifndef __SPU__
+ 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 if (m_damping > SIMD_EPSILON) // no motor: do a little damping
+ {
+ btVector3 angVelA; bodyA.getAngularVelocity(angVelA);
+ btVector3 angVelB; bodyB.getAngularVelocity(angVelB);
+ 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);
+ }
+ }
+ }
+#else
+btAssert(0);
+#endif //__SPU__
+}
+
+
+
+
+void btConeTwistConstraint::updateRHS(btScalar timeStep)
+{
+ (void)timeStep;
+
+}
+
+
+#ifndef __SPU__
+void btConeTwistConstraint::calcAngleInfo()
+{
+ m_swingCorrection = btScalar(0.);
+ m_twistLimitSign = btScalar(0.);
+ m_solveTwistLimit = false;
+ m_solveSwingLimit = false;
+
+ btVector3 b1Axis1,b1Axis2,b1Axis3;
+ btVector3 b2Axis1,b2Axis2;
+
+ b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
+ b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
+
+ btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
+
+ btScalar swx=btScalar(0.),swy = btScalar(0.);
+ btScalar thresh = btScalar(10.);
+ btScalar fact;
+
+ // Get Frame into world space
+ if (m_swingSpan1 >= btScalar(0.05f))
+ {
+ b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
+ 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);
+ swx = b2Axis1.dot(b1Axis1);
+ swy = b2Axis1.dot(b1Axis3);
+ swing2 = btAtan2Fast(swy, swx);
+ fact = (swy*swy + swx*swx) * thresh * thresh;
+ fact = fact / (fact + btScalar(1.0));
+ swing2 *= fact;
+ }
+
+ btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
+ btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
+ btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
+
+ if (EllipseAngle > 1.0f)
+ {
+ 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;
+ }
+
+ // Twist limits
+ if (m_twistSpan >= btScalar(0.))
+ {
+ btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
+ 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)) ? 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();
+ }
+ }
+}
+#endif //__SPU__
+
+static btVector3 vTwist(1,0,0); // twist axis in constraint's space
+
+
+
+void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+{
+ m_swingCorrection = btScalar(0.);
+ m_twistLimitSign = btScalar(0.);
+ m_solveTwistLimit = false;
+ m_solveSwingLimit = false;
+ // compute rotation of A wrt B (in constraint space)
+ if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
+ { // it is assumed that setMotorTarget() was alredy called
+ // and motor target m_qTarget is within constraint limits
+ // TODO : split rotation to pure swing and pure twist
+ // compute desired transforms in world
+ btTransform trPose(m_qTarget);
+ btTransform trA = transA * m_rbAFrame;
+ btTransform trB = transB * m_rbBFrame;
+ btTransform trDeltaAB = trB * trPose * trA.inverse();
+ btQuaternion qDeltaAB = trDeltaAB.getRotation();
+ btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
+ m_swingAxis = swingAxis;
+ m_swingAxis.normalize();
+ m_swingCorrection = qDeltaAB.getAngle();
+ if(!btFuzzyZero(m_swingCorrection))
+ {
+ m_solveSwingLimit = true;
+ }
+ return;
+ }
+
+
+ {
+ // compute rotation of A wrt B (in constraint space)
+ btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
+ btQuaternion qB = transB.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.) /
+ (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
+ computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
+ }
+ }
+ 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 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+ btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+ btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
+ btVector3 ivB = transB.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();
+ }
+ }
+
+ if (m_twistSpan >= btScalar(0.f))
+ {
+ btVector3 twistAxis;
+ computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
+
+ if (m_twistAngle > m_twistSpan*m_limitSoftness)
+ {
+ m_solveTwistLimit = true;
+
+ 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);
+ }
+
+ // 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.) /
+ (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
+ computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
+ }
+
+ if (m_solveSwingLimit)
+ m_twistAxisA = quatRotate(qA, -twistAxis);
+ }
+ else
+ {
+ m_twistAngle = btScalar(0.f);
+ }
+ }
+}
+
+
+
+// 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;
+ }
+}
+
+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);
+ }
+
+ // 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);
+}
+
+// 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();
+
+ if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+ {
+ qMinTwist = operator-(qTwist);
+ twistAngle = qMinTwist.getAngle();
+ }
+ if (twistAngle < 0)
+ {
+ // this should never happen
+ int wtf = 0; wtf = wtf;
+ }
+
+ vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+ if (twistAngle > SIMD_EPSILON)
+ vTwistAxis.normalize();
+}
+
+
+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;
+
+ // 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);
+}
+
+
+void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
+{
+ m_qTarget = q;
+
+ // clamp motor target to within limits
+ {
+ btScalar softness = 1.f;//m_limitSoftness;
+
+ // 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();
+
+ // 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);
+
+ 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;
+ }
+}
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btContactConstraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btContactConstraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btContactConstraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btContactConstraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,141 @@
+/*
+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 "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
+btContactConstraint::btContactConstraint()
+:btTypedConstraint(CONTACT_CONSTRAINT_TYPE)
+{
+}
+
+btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
+:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
+ m_contactManifold(*contactManifold)
+{
+
+}
+
+btContactConstraint::~btContactConstraint()
+{
+
+}
+
+void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
+{
+ m_contactManifold = *contactManifold;
+}
+
+void btContactConstraint::getInfo1 (btConstraintInfo1* info)
+{
+
+}
+
+void btContactConstraint::getInfo2 (btConstraintInfo2* info)
+{
+
+}
+
+void btContactConstraint::buildJacobian()
+{
+
+}
+
+void btContactConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+
+}
+
+
+
+
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
+#define ASSERT2 btAssert
+
+#define USE_INTERNAL_APPLY_IMPULSE 1
+
+
+//bilateral constraint between two dynamic objects
+void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
+ btRigidBody& body2, const btVector3& pos2,
+ btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
+{
+ (void)timeStep;
+ (void)distance;
+
+
+ btScalar normalLenSqr = normal.length2();
+ ASSERT2(btFabs(normalLenSqr) < btScalar(1.1));
+ if (normalLenSqr > btScalar(1.1))
+ {
+ impulse = btScalar(0.);
+ return;
+ }
+ btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
+ btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+ //this jacobian entry could be re-used for all iterations
+
+ btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+ btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+ btVector3 vel = vel1 - vel2;
+
+
+ btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+ body2.getCenterOfMassTransform().getBasis().transpose(),
+ rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
+ body2.getInvInertiaDiagLocal(),body2.getInvMass());
+
+ btScalar jacDiagAB = jac.getDiagonal();
+ btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
+
+ btScalar rel_vel = jac.getRelativeVelocity(
+ body1.getLinearVelocity(),
+ body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
+ body2.getLinearVelocity(),
+ body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
+ btScalar a;
+ a=jacDiagABInv;
+
+
+ rel_vel = normal.dot(vel);
+
+ //todo: move this into proper structure
+ btScalar contactDamping = btScalar(0.2);
+
+#ifdef ONLY_USE_LINEAR_MASS
+ btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
+ impulse = - contactDamping * rel_vel * massTerm;
+#else
+ btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
+ impulse = velocityImpulse;
+#endif
+}
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btContactProcessing.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btContactProcessing.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btContactProcessing.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btContactProcessing.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,181 @@
+
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.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 "BulletCollision/Gimpact/btContactProcessing.h"
+
+#define MAX_COINCIDENT 8
+
+struct CONTACT_KEY_TOKEN
+{
+ unsigned int m_key;
+ int m_value;
+ CONTACT_KEY_TOKEN()
+ {
+ }
+
+ CONTACT_KEY_TOKEN(unsigned int key,int token)
+ {
+ m_key = key;
+ m_value = token;
+ }
+
+ CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken)
+ {
+ m_key = rtoken.m_key;
+ m_value = rtoken.m_value;
+ }
+
+ inline bool operator <(const CONTACT_KEY_TOKEN& other) const
+ {
+ return (m_key < other.m_key);
+ }
+
+ inline bool operator >(const CONTACT_KEY_TOKEN& other) const
+ {
+ return (m_key > other.m_key);
+ }
+
+};
+
+class CONTACT_KEY_TOKEN_COMP
+{
+ public:
+
+ bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b )
+ {
+ return ( a < b );
+ }
+};
+
+
+void btContactArray::merge_contacts(
+ const btContactArray & contacts, bool normal_contact_average)
+{
+ clear();
+
+ int i;
+ if(contacts.size()==0) return;
+
+
+ if(contacts.size()==1)
+ {
+ push_back(contacts[0]);
+ return;
+ }
+
+ btAlignedObjectArray<CONTACT_KEY_TOKEN> keycontacts;
+
+ keycontacts.reserve(contacts.size());
+
+ //fill key contacts
+
+ for ( i = 0;i<contacts.size() ;i++ )
+ {
+ keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i));
+ }
+
+ //sort keys
+ keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP());
+
+ // Merge contacts
+ int coincident_count=0;
+ btVector3 coincident_normals[MAX_COINCIDENT];
+
+ unsigned int last_key = keycontacts[0].m_key;
+ unsigned int key = 0;
+
+ push_back(contacts[keycontacts[0].m_value]);
+
+ GIM_CONTACT * pcontact = &(*this)[0];
+
+ for( i=1;i<keycontacts.size();i++)
+ {
+ key = keycontacts[i].m_key;
+ const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
+
+ if(last_key == key)//same points
+ {
+ //merge contact
+ if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
+ {
+ *pcontact = *scontact;
+ coincident_count = 0;
+ }
+ else if(normal_contact_average)
+ {
+ if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
+ {
+ if(coincident_count<MAX_COINCIDENT)
+ {
+ coincident_normals[coincident_count] = scontact->m_normal;
+ coincident_count++;
+ }
+ }
+ }
+ }
+ else
+ {//add new contact
+
+ if(normal_contact_average && coincident_count>0)
+ {
+ pcontact->interpolate_normals(coincident_normals,coincident_count);
+ coincident_count = 0;
+ }
+
+ push_back(*scontact);
+ pcontact = &(*this)[this->size()-1];
+ }
+ last_key = key;
+ }
+}
+
+void btContactArray::merge_contacts_unique(const btContactArray & contacts)
+{
+ clear();
+
+ if(contacts.size()==0) return;
+
+ if(contacts.size()==1)
+ {
+ push_back(contacts[0]);
+ return;
+ }
+
+ GIM_CONTACT average_contact = contacts[0];
+
+ for (int i=1;i<contacts.size() ;i++ )
+ {
+ average_contact.m_point += contacts[i].m_point;
+ average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
+ }
+
+ //divide
+ btScalar divide_average = 1.0f/((btScalar)contacts.size());
+
+ average_contact.m_point *= divide_average;
+
+ average_contact.m_normal *= divide_average;
+
+ average_contact.m_depth = average_contact.m_normal.length();
+
+ average_contact.m_normal /= average_contact.m_depth;
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btContinuousConvexCollision.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btContinuousConvexCollision.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btContinuousConvexCollision.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btContinuousConvexCollision.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,236 @@
+/*
+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 "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
+
+
+
+btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
+:m_simplexSolver(simplexSolver),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
+/// You don't want your game ever to lock-up.
+#define MAX_ITERATIONS 64
+
+bool btContinuousConvexCollision::calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result)
+{
+
+ m_simplexSolver->reset();
+
+ /// compute linear and angular velocity for this interval, to interpolate
+ btVector3 linVelA,angVelA,linVelB,angVelB;
+ btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
+ btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
+
+
+ btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
+ btScalar boundingRadiusB = m_convexB->getAngularMotionDisc();
+
+ btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
+ btVector3 relLinVel = (linVelB-linVelA);
+
+ btScalar relLinVelocLength = (linVelB-linVelA).length();
+
+ if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
+ return false;
+
+
+ btScalar radius = btScalar(0.001);
+
+ btScalar lambda = btScalar(0.);
+ btVector3 v(1,0,0);
+
+ int maxIter = MAX_ITERATIONS;
+
+ btVector3 n;
+ n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ bool hasResult = false;
+ btVector3 c;
+
+ btScalar lastLambda = lambda;
+ //btScalar epsilon = btScalar(0.001);
+
+ int numIter = 0;
+ //first solution, using GJK
+
+
+ btTransform identityTrans;
+ identityTrans.setIdentity();
+
+ btSphereShape raySphere(btScalar(0.0));
+ raySphere.setMargin(btScalar(0.));
+
+
+// result.drawCoordSystem(sphereTr);
+
+ btPointCollector pointCollector1;
+
+ {
+
+ btGjkPairDetector gjk(m_convexA,m_convexB,m_convexA->getShapeType(),m_convexB->getShapeType(),m_convexA->getMargin(),m_convexB->getMargin(),m_simplexSolver,m_penetrationDepthSolver);
+ btGjkPairDetector::ClosestPointInput input;
+
+ //we don't use margins during CCD
+ // gjk.setIgnoreMargin(true);
+
+ input.m_transformA = fromA;
+ input.m_transformB = fromB;
+ gjk.getClosestPoints(input,pointCollector1,0);
+
+ hasResult = pointCollector1.m_hasResult;
+ c = pointCollector1.m_pointInWorld;
+ }
+
+ if (hasResult)
+ {
+ btScalar dist;
+ dist = pointCollector1.m_distance;
+ n = pointCollector1.m_normalOnBInWorld;
+
+ btScalar projectedLinearVelocity = relLinVel.dot(n);
+
+ //not close enough
+ while (dist > radius)
+ {
+ if (result.m_debugDrawer)
+ {
+ result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
+ }
+ numIter++;
+ if (numIter > maxIter)
+ {
+ return false; //todo: report a failure
+ }
+ btScalar dLambda = btScalar(0.);
+
+ projectedLinearVelocity = relLinVel.dot(n);
+
+ //calculate safe moving fraction from distance / (linear+rotational velocity)
+
+ //btScalar clippedDist = GEN_min(angularConservativeRadius,dist);
+ //btScalar clippedDist = dist;
+
+ //don't report time of impact for motion away from the contact normal (or causes minor penetration)
+ if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
+ return false;
+
+ dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
+
+
+
+ lambda = lambda + dLambda;
+
+ if (lambda > btScalar(1.))
+ return false;
+
+ if (lambda < btScalar(0.))
+ return false;
+
+
+ //todo: next check with relative epsilon
+ if (lambda <= lastLambda)
+ {
+ return false;
+ //n.setValue(0,0,0);
+ break;
+ }
+ lastLambda = lambda;
+
+
+
+ //interpolate to next lambda
+ btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
+
+ btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
+ btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
+ relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
+
+ if (result.m_debugDrawer)
+ {
+ result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0));
+ }
+
+ result.DebugDraw( lambda );
+
+ btPointCollector pointCollector;
+ btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
+ btGjkPairDetector::ClosestPointInput input;
+ input.m_transformA = interpolatedTransA;
+ input.m_transformB = interpolatedTransB;
+ gjk.getClosestPoints(input,pointCollector,0);
+ if (pointCollector.m_hasResult)
+ {
+ if (pointCollector.m_distance < btScalar(0.))
+ {
+ //degenerate ?!
+ result.m_fraction = lastLambda;
+ n = pointCollector.m_normalOnBInWorld;
+ result.m_normal=n;//.setValue(1,1,1);// = n;
+ result.m_hitPoint = pointCollector.m_pointInWorld;
+ return true;
+ }
+ c = pointCollector.m_pointInWorld;
+ n = pointCollector.m_normalOnBInWorld;
+ dist = pointCollector.m_distance;
+ } else
+ {
+ //??
+ return false;
+ }
+
+
+ }
+
+ if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
+ return false;
+
+ result.m_fraction = lambda;
+ result.m_normal = n;
+ result.m_hitPoint = c;
+ return true;
+ }
+
+ return false;
+
+/*
+//todo:
+ //if movement away from normal, discard result
+ btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
+ if (result.m_fraction < btScalar(1.))
+ {
+ if (move.dot(result.m_normal) <= btScalar(0.))
+ {
+ }
+ }
+*/
+
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btContinuousDynamicsWorld.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btContinuousDynamicsWorld.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btContinuousDynamicsWorld.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btContinuousDynamicsWorld.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,196 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BulletDynamics/Dynamics/btContinuousDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+//collision detection
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+
+//rigidbody & constraints
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+
+
+#include <stdio.h>
+
+btContinuousDynamicsWorld::btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+{
+}
+
+btContinuousDynamicsWorld::~btContinuousDynamicsWorld()
+{
+}
+
+
+void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
+{
+
+ startProfiling(timeStep);
+
+ if(0 != m_internalPreTickCallback) {
+ (*m_internalPreTickCallback)(this, timeStep);
+ }
+
+
+ ///update aabbs information
+ updateAabbs();
+ //static int frame=0;
+// printf("frame %d\n",frame++);
+
+ ///apply gravity, predict motion
+ predictUnconstraintMotion(timeStep);
+
+ btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
+ dispatchInfo.m_timeStep = timeStep;
+ dispatchInfo.m_stepCount = 0;
+ dispatchInfo.m_debugDraw = getDebugDrawer();
+
+ ///perform collision detection
+ performDiscreteCollisionDetection();
+
+ calculateSimulationIslands();
+
+
+ getSolverInfo().m_timeStep = timeStep;
+
+
+
+ ///solve contact and other joint constraints
+ solveConstraints(getSolverInfo());
+
+ ///CallbackTriggers();
+ calculateTimeOfImpacts(timeStep);
+
+ btScalar toi = dispatchInfo.m_timeOfImpact;
+// if (toi < 1.f)
+// printf("toi = %f\n",toi);
+ if (toi < 0.f)
+ printf("toi = %f\n",toi);
+
+
+ ///integrate transforms
+ integrateTransforms(timeStep * toi);
+
+ ///update vehicle simulation
+ updateActions(timeStep);
+
+ updateActivationState( timeStep );
+
+ if(0 != m_internalTickCallback) {
+ (*m_internalTickCallback)(this, timeStep);
+ }
+}
+
+void btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep)
+{
+ ///these should be 'temporal' aabbs!
+ updateTemporalAabbs(timeStep);
+
+ ///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually.
+ ///so we handle the case moving versus static properly, and we cheat for moving versus moving
+ btScalar toi = 1.f;
+
+
+ btDispatcherInfo& dispatchInfo = getDispatchInfo();
+ dispatchInfo.m_timeStep = timeStep;
+ dispatchInfo.m_timeOfImpact = 1.f;
+ dispatchInfo.m_stepCount = 0;
+ dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS;
+
+ ///calculate time of impact for overlapping pairs
+
+
+ btDispatcher* dispatcher = getDispatcher();
+ if (dispatcher)
+ dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
+
+ toi = dispatchInfo.m_timeOfImpact;
+
+ dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE;
+
+}
+
+void btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep)
+{
+
+ btVector3 temporalAabbMin,temporalAabbMax;
+
+ for ( int i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+
+ btRigidBody* body = btRigidBody::upcast(colObj);
+ if (body)
+ {
+ body->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),temporalAabbMin,temporalAabbMax);
+ const btVector3& linvel = body->getLinearVelocity();
+
+ //make the AABB temporal
+ btScalar temporalAabbMaxx = temporalAabbMax.getX();
+ btScalar temporalAabbMaxy = temporalAabbMax.getY();
+ btScalar temporalAabbMaxz = temporalAabbMax.getZ();
+ btScalar temporalAabbMinx = temporalAabbMin.getX();
+ btScalar temporalAabbMiny = temporalAabbMin.getY();
+ btScalar temporalAabbMinz = temporalAabbMin.getZ();
+
+ // add linear motion
+ btVector3 linMotion = linvel*timeStep;
+
+ if (linMotion.x() > 0.f)
+ temporalAabbMaxx += linMotion.x();
+ else
+ temporalAabbMinx += linMotion.x();
+ if (linMotion.y() > 0.f)
+ temporalAabbMaxy += linMotion.y();
+ else
+ temporalAabbMiny += linMotion.y();
+ if (linMotion.z() > 0.f)
+ temporalAabbMaxz += linMotion.z();
+ else
+ temporalAabbMinz += linMotion.z();
+
+ //add conservative angular motion
+ btScalar angularMotion(0);// = angvel.length() * GetAngularMotionDisc() * timeStep;
+ btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
+ temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
+ temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
+
+ temporalAabbMin -= angularMotion3d;
+ temporalAabbMax += angularMotion3d;
+
+ m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1);
+ }
+ }
+
+ //update aabb (of all moved objects)
+
+ m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
+
+
+
+}
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvex2dConvex2dAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvex2dConvex2dAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvex2dConvex2dAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvex2dConvex2dAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,247 @@
+/*
+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 "BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h"
+
+//#include <stdio.h>
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+
+
+
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+
+
+btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+{
+ m_numPerturbationIterations = 0;
+ m_minimumPointsPerturbationThreshold = 3;
+ m_simplexSolver = simplexSolver;
+ m_pdSolver = pdSolver;
+}
+
+btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
+{
+}
+
+btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(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_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+{
+ (void)body0;
+ (void)body1;
+}
+
+
+
+
+btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
+{
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr)
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ }
+}
+
+void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+{
+ m_lowLevelOfDetail = useLowLevel;
+}
+
+
+
+extern btScalar gContactBreakingThreshold;
+
+
+//
+// Convex-Convex collision algorithm
+//
+void btConvex2dConvex2dAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+ if (!m_manifoldPtr)
+ {
+ //swapped?
+ m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+ m_ownManifold = true;
+ }
+ resultOut->setPersistentManifold(m_manifoldPtr);
+
+ //comment-out next line to test multi-contact generation
+ //resultOut->getPersistentManifold()->clearManifold();
+
+
+ btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
+ btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
+
+ btVector3 normalOnB;
+ btVector3 pointOnBWorld;
+
+ {
+
+
+ btGjkPairDetector::ClosestPointInput input;
+
+ btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
+ //TODO: if (dispatchInfo.m_useContinuous)
+ gjkPairDetector.setMinkowskiA(min0);
+ 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;
+ input.m_transformA = body0->getWorldTransform();
+ input.m_transformB = body1->getWorldTransform();
+
+ gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+ btVector3 v0,v1;
+ btVector3 sepNormalWorldSpace;
+
+ }
+
+ if (m_ownManifold)
+ {
+ resultOut->refreshContactPoints();
+ }
+
+}
+
+
+
+
+btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)resultOut;
+ (void)dispatchInfo;
+ ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
+
+ ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
+ ///col0->m_worldTransform,
+ btScalar resultFraction = btScalar(1.);
+
+
+ btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
+ btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
+
+ if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
+ squareMot1 < col1->getCcdSquareMotionThreshold())
+ return resultFraction;
+
+
+ //An adhoc way of testing the Continuous Collision Detection algorithms
+ //One object is approximated as a sphere, to simplify things
+ //Starting in penetration should report no time of impact
+ //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
+ //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
+
+
+ /// Convex0 against sphere for Convex1
+ {
+ btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
+
+ btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+ btConvexCast::CastResult result;
+ btVoronoiSimplexSolver voronoiSimplex;
+ //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+ ///Simplification, one object is simplified as a sphere
+ btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
+ //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+ if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+ col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+ {
+
+ //store result.m_fraction in both bodies
+
+ if (col0->getHitFraction()> result.m_fraction)
+ col0->setHitFraction( result.m_fraction );
+
+ if (col1->getHitFraction() > result.m_fraction)
+ col1->setHitFraction( result.m_fraction);
+
+ if (resultFraction > result.m_fraction)
+ resultFraction = result.m_fraction;
+
+ }
+
+
+
+
+ }
+
+ /// Sphere (for convex0) against Convex1
+ {
+ btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
+
+ btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+ btConvexCast::CastResult result;
+ btVoronoiSimplexSolver voronoiSimplex;
+ //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+ ///Simplification, one object is simplified as a sphere
+ btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
+ //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+ if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+ col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+ {
+
+ //store result.m_fraction in both bodies
+
+ if (col0->getHitFraction() > result.m_fraction)
+ col0->setHitFraction( result.m_fraction);
+
+ if (col1->getHitFraction() > result.m_fraction)
+ col1->setHitFraction( result.m_fraction);
+
+ if (resultFraction > result.m_fraction)
+ resultFraction = result.m_fraction;
+
+ }
+ }
+
+ return resultFraction;
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvex2dShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvex2dShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvex2dShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvex2dShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,92 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btConvex2dShape.h"
+
+btConvex2dShape::btConvex2dShape( btConvexShape* convexChildShape):
+btConvexShape (), m_childConvexShape(convexChildShape)
+{
+ m_shapeType = CONVEX_2D_SHAPE_PROXYTYPE;
+}
+
+btConvex2dShape::~btConvex2dShape()
+{
+}
+
+
+
+btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+ return m_childConvexShape->localGetSupportingVertexWithoutMargin(vec);
+}
+
+void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+ m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors);
+}
+
+
+btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec)const
+{
+ return m_childConvexShape->localGetSupportingVertex(vec);
+}
+
+
+void btConvex2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ ///this linear upscaling is not realistic, but we don't deal with large mass ratios...
+ m_childConvexShape->calculateLocalInertia(mass,inertia);
+}
+
+
+ ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void btConvex2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ m_childConvexShape->getAabb(t,aabbMin,aabbMax);
+}
+
+void btConvex2dShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax);
+}
+
+void btConvex2dShape::setLocalScaling(const btVector3& scaling)
+{
+ m_childConvexShape->setLocalScaling(scaling);
+}
+
+const btVector3& btConvex2dShape::getLocalScaling() const
+{
+ return m_childConvexShape->getLocalScaling();
+}
+
+void btConvex2dShape::setMargin(btScalar margin)
+{
+ m_childConvexShape->setMargin(margin);
+}
+btScalar btConvex2dShape::getMargin() const
+{
+ return m_childConvexShape->getMargin();
+}
+
+int btConvex2dShape::getNumPreferredPenetrationDirections() const
+{
+ return m_childConvexShape->getNumPreferredPenetrationDirections();
+}
+
+void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+{
+ m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexCast.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexCast.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexCast.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexCast.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,20 @@
+/*
+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 "BulletCollision/NarrowPhaseCollision/btConvexCast.h"
+
+btConvexCast::~btConvexCast()
+{
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexConcaveCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexConcaveCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexConcaveCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexConcaveCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,317 @@
+/*
+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 "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionShapes/btConcaveShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+
+btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+: btActivatingCollisionAlgorithm(ci,body0,body1),
+m_isSwapped(isSwapped),
+m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
+{
+}
+
+btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
+{
+}
+
+void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
+{
+ if (m_btConvexTriangleCallback.m_manifoldPtr)
+ {
+ manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
+ }
+}
+
+
+btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
+ m_dispatcher(dispatcher),
+ m_dispatchInfoPtr(0)
+{
+ m_convexBody = 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();
+}
+
+btConvexTriangleCallback::~btConvexTriangleCallback()
+{
+ clearCache();
+ m_dispatcher->releaseManifold( m_manifoldPtr );
+
+}
+
+
+void btConvexTriangleCallback::clearCache()
+{
+ m_dispatcher->clearManifold(m_manifoldPtr);
+}
+
+
+
+void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+{
+
+ //just for debugging purposes
+ //printf("triangle %d",m_triangleCount++);
+
+
+ //aabb filter is already applied!
+
+ btCollisionAlgorithmConstructionInfo ci;
+ ci.m_dispatcher1 = m_dispatcher;
+
+ btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
+
+
+
+ ///debug drawing of the overlapping triangles
+ if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
+ {
+ btVector3 color(255,255,0);
+ btTransform& tr = ob->getWorldTransform();
+ m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
+ m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
+ m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
+
+ //btVector3 center = triangle[0] + triangle[1]+triangle[2];
+ //center *= btScalar(0.333333);
+ //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color);
+ //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color);
+ //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color);
+
+ }
+
+
+ //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
+
+ if (m_convexBody->getCollisionShape()->isConvex())
+ {
+ btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
+ tm.setMargin(m_collisionMarginTriangle);
+
+ btCollisionShape* tmpShape = ob->getCollisionShape();
+ ob->internalSetTemporaryCollisionShape( &tm );
+
+ btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,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->setShapeIdentifiersB(partId,triangleIndex);
+
+// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+ colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+ colAlgo->~btCollisionAlgorithm();
+ ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
+ ob->internalSetTemporaryCollisionShape( tmpShape);
+ }
+
+
+}
+
+
+
+void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ m_dispatchInfoPtr = &dispatchInfo;
+ m_collisionMarginTriangle = collisionMarginTriangle;
+ m_resultOut = resultOut;
+
+ //recalc aabbs
+ btTransform convexInTriangleSpace;
+ convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform();
+ btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape());
+ //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
+ convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
+ btScalar extraMargin = collisionMarginTriangle;
+ btVector3 extra(extraMargin,extraMargin,extraMargin);
+
+ m_aabbMax += extra;
+ m_aabbMin -= extra;
+
+}
+
+void btConvexConcaveCollisionAlgorithm::clearCache()
+{
+ m_btConvexTriangleCallback.clearCache();
+
+}
+
+void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+
+ btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
+ btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+ if (triBody->getCollisionShape()->isConcave())
+ {
+
+
+ btCollisionObject* triOb = triBody;
+ btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
+
+ if (convexBody->getCollisionShape()->isConvex())
+ {
+ btScalar collisionMarginTriangle = concaveShape->getMargin();
+
+ resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+ m_btConvexTriangleCallback.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_btConvexTriangleCallback.m_manifoldPtr);
+
+ m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
+
+ concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
+
+ resultOut->refreshContactPoints();
+
+ }
+
+ }
+
+}
+
+
+btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)resultOut;
+ (void)dispatchInfo;
+ btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
+ btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+
+ //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
+
+ //only perform CCD above a certain threshold, this prevents blocking on the long run
+ //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
+ btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
+ if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
+ {
+ return btScalar(1.);
+ }
+
+ //const btVector3& from = convexbody->m_worldTransform.getOrigin();
+ //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
+ //todo: only do if the motion exceeds the 'radius'
+
+ btTransform triInv = triBody->getWorldTransform().inverse();
+ btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
+ btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
+
+ struct LocalTriangleSphereCastCallback : public btTriangleCallback
+ {
+ btTransform m_ccdSphereFromTrans;
+ btTransform m_ccdSphereToTrans;
+ btTransform m_meshTransform;
+
+ btScalar m_ccdSphereRadius;
+ btScalar m_hitFraction;
+
+
+ LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
+ :m_ccdSphereFromTrans(from),
+ m_ccdSphereToTrans(to),
+ m_ccdSphereRadius(ccdSphereRadius),
+ m_hitFraction(hitFraction)
+ {
+ }
+
+
+ virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+ {
+ (void)partId;
+ (void)triangleIndex;
+ //do a swept sphere for now
+ btTransform ident;
+ ident.setIdentity();
+ btConvexCast::CastResult castResult;
+ castResult.m_fraction = m_hitFraction;
+ btSphereShape pointShape(m_ccdSphereRadius);
+ btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
+ btVoronoiSimplexSolver simplexSolver;
+ btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
+ //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
+ //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+ //local space?
+
+ if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
+ ident,ident,castResult))
+ {
+ if (m_hitFraction > castResult.m_fraction)
+ m_hitFraction = castResult.m_fraction;
+ }
+
+ }
+
+ };
+
+
+
+
+
+ if (triBody->getCollisionShape()->isConcave())
+ {
+ btVector3 rayAabbMin = convexFromLocal.getOrigin();
+ rayAabbMin.setMin(convexToLocal.getOrigin());
+ btVector3 rayAabbMax = convexFromLocal.getOrigin();
+ rayAabbMax.setMax(convexToLocal.getOrigin());
+ btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
+ rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+ rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+
+ btScalar curHitFraction = btScalar(1.); //is this available?
+ LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
+ convexbody->getCcdSweptSphereRadius(),curHitFraction);
+
+ raycastCallback.m_hitFraction = convexbody->getHitFraction();
+
+ btCollisionObject* concavebody = triBody;
+
+ btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
+
+ if (triangleMesh)
+ {
+ triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
+ }
+
+
+
+ if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
+ {
+ convexbody->setHitFraction( raycastCallback.m_hitFraction);
+ return raycastCallback.m_hitFraction;
+ }
+ }
+
+ return btScalar(1.);
+
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexConvexAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexConvexAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexConvexAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexConvexAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,565 @@
+/*
+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.
+*/
+
+///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
+///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
+///with reproduction case
+//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
+
+#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
+
+//#include <stdio.h>
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+
+
+
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+
+
+
+///////////
+
+
+
+static SIMD_FORCE_INLINE void segmentsClosestPoints(
+ btVector3& ptsVector,
+ btVector3& offsetA,
+ btVector3& offsetB,
+ btScalar& tA, btScalar& tB,
+ const btVector3& translation,
+ const btVector3& dirA, btScalar hlenA,
+ const btVector3& dirB, btScalar hlenB )
+{
+ // compute the parameters of the closest points on each line segment
+
+ btScalar dirA_dot_dirB = btDot(dirA,dirB);
+ btScalar dirA_dot_trans = btDot(dirA,translation);
+ btScalar dirB_dot_trans = btDot(dirB,translation);
+
+ btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
+
+ if ( denom == 0.0f ) {
+ tA = 0.0f;
+ } else {
+ tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
+ if ( tA < -hlenA )
+ tA = -hlenA;
+ else if ( tA > hlenA )
+ tA = hlenA;
+ }
+
+ tB = tA * dirA_dot_dirB - dirB_dot_trans;
+
+ if ( tB < -hlenB ) {
+ tB = -hlenB;
+ tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+ if ( tA < -hlenA )
+ tA = -hlenA;
+ else if ( tA > hlenA )
+ tA = hlenA;
+ } else if ( tB > hlenB ) {
+ tB = hlenB;
+ tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+ if ( tA < -hlenA )
+ tA = -hlenA;
+ else if ( tA > hlenA )
+ tA = hlenA;
+ }
+
+ // compute the closest points relative to segment centers.
+
+ offsetA = dirA * tA;
+ offsetB = dirB * tB;
+
+ ptsVector = translation - offsetA + offsetB;
+}
+
+
+static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
+ btVector3& normalOnB,
+ btVector3& pointOnB,
+ btScalar capsuleLengthA,
+ btScalar capsuleRadiusA,
+ btScalar capsuleLengthB,
+ btScalar capsuleRadiusB,
+ int capsuleAxisA,
+ int capsuleAxisB,
+ const btTransform& transformA,
+ const btTransform& transformB,
+ btScalar distanceThreshold )
+{
+ btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
+ btVector3 translationA = transformA.getOrigin();
+ btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
+ btVector3 translationB = transformB.getOrigin();
+
+ // translation between centers
+
+ btVector3 translation = translationB - translationA;
+
+ // compute the closest points of the capsule line segments
+
+ btVector3 ptsVector; // the vector between the closest points
+
+ btVector3 offsetA, offsetB; // offsets from segment centers to their closest points
+ btScalar tA, tB; // parameters on line segment
+
+ segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
+ directionA, capsuleLengthA, directionB, capsuleLengthB );
+
+ btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
+
+ if ( distance > distanceThreshold )
+ return distance;
+
+ btScalar lenSqr = ptsVector.length2();
+ if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
+ {
+ //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
+ btVector3 q;
+ btPlaneSpace1(directionA,normalOnB,q);
+ } else
+ {
+ // compute the contact normal
+ normalOnB = ptsVector*-btRecipSqrt(lenSqr);
+ }
+ pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
+
+ return distance;
+}
+
+
+
+
+
+
+
+//////////
+
+
+
+
+
+btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+{
+ m_numPerturbationIterations = 0;
+ m_minimumPointsPerturbationThreshold = 3;
+ m_simplexSolver = simplexSolver;
+ m_pdSolver = pdSolver;
+}
+
+btConvexConvexAlgorithm::CreateFunc::~CreateFunc()
+{
+}
+
+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),
+#ifdef USE_SEPDISTANCE_UTIL2
+m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
+ (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
+#endif
+m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+{
+ (void)body0;
+ (void)body1;
+}
+
+
+
+
+btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
+{
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr)
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ }
+}
+
+void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+{
+ m_lowLevelOfDetail = 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
+//
+void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+ if (!m_manifoldPtr)
+ {
+ //swapped?
+ m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+ m_ownManifold = true;
+ }
+ resultOut->setPersistentManifold(m_manifoldPtr);
+
+ //comment-out next line to test multi-contact generation
+ //resultOut->getPersistentManifold()->clearManifold();
+
+
+ btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
+ btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
+
+ btVector3 normalOnB;
+ btVector3 pointOnBWorld;
+#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
+ if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
+ {
+ btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
+ btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
+ btVector3 localScalingA = capsuleA->getLocalScaling();
+ btVector3 localScalingB = capsuleB->getLocalScaling();
+
+ btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+
+ btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
+ capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
+ body0->getWorldTransform(),body1->getWorldTransform(),threshold);
+
+ if (dist<threshold)
+ {
+ btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
+ resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);
+ }
+ resultOut->refreshContactPoints();
+ return;
+ }
+#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
+
+
+#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)
+ gjkPairDetector.setMinkowskiA(min0);
+ gjkPairDetector.setMinkowskiB(min1);
+
+#ifdef USE_SEPDISTANCE_UTIL2
+ if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+ {
+ input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
+ } 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);
+
+ btVector3 v0,v1;
+ btVector3 sepNormalWorldSpace;
+
+
+#ifdef USE_SEPDISTANCE_UTIL2
+ btScalar sepDist = 0.f;
+ if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+ {
+ sepDist = gjkPairDetector.getCachedSeparatingDistance();
+ if (sepDist>SIMD_EPSILON)
+ {
+ sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
+ //now perturbe directions to get multiple contact points
+ sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
+ btPlaneSpace1(sepNormalWorldSpace,v0,v1);
+ }
+ }
+#endif //USE_SEPDISTANCE_UTIL2
+
+ //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
+
+ //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;i<m_numPerturbationIterations;i++)
+ {
+ btQuaternion perturbeRot(v0,perturbeAngle);
+ btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
+ btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
+
+
+ if (perturbeA)
+ {
+ input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().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 && (sepDist>SIMD_EPSILON))
+ {
+ m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
+ }
+#endif //USE_SEPDISTANCE_UTIL2
+
+
+ }
+
+ if (m_ownManifold)
+ {
+ resultOut->refreshContactPoints();
+ }
+
+}
+
+
+
+bool disableCcd = false;
+btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)resultOut;
+ (void)dispatchInfo;
+ ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
+
+ ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
+ ///col0->m_worldTransform,
+ btScalar resultFraction = btScalar(1.);
+
+
+ btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
+ btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
+
+ if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
+ squareMot1 < col1->getCcdSquareMotionThreshold())
+ return resultFraction;
+
+ if (disableCcd)
+ return btScalar(1.);
+
+
+ //An adhoc way of testing the Continuous Collision Detection algorithms
+ //One object is approximated as a sphere, to simplify things
+ //Starting in penetration should report no time of impact
+ //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
+ //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
+
+
+ /// Convex0 against sphere for Convex1
+ {
+ btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
+
+ btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+ btConvexCast::CastResult result;
+ btVoronoiSimplexSolver voronoiSimplex;
+ //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+ ///Simplification, one object is simplified as a sphere
+ btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
+ //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+ if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+ col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+ {
+
+ //store result.m_fraction in both bodies
+
+ if (col0->getHitFraction()> result.m_fraction)
+ col0->setHitFraction( result.m_fraction );
+
+ if (col1->getHitFraction() > result.m_fraction)
+ col1->setHitFraction( result.m_fraction);
+
+ if (resultFraction > result.m_fraction)
+ resultFraction = result.m_fraction;
+
+ }
+
+
+
+
+ }
+
+ /// Sphere (for convex0) against Convex1
+ {
+ btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
+
+ btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+ btConvexCast::CastResult result;
+ btVoronoiSimplexSolver voronoiSimplex;
+ //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+ ///Simplification, one object is simplified as a sphere
+ btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
+ //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+ if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+ col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+ {
+
+ //store result.m_fraction in both bodies
+
+ if (col0->getHitFraction() > result.m_fraction)
+ col0->setHitFraction( result.m_fraction);
+
+ if (col1->getHitFraction() > result.m_fraction)
+ col1->setHitFraction( result.m_fraction);
+
+ if (resultFraction > result.m_fraction)
+ resultFraction = result.m_fraction;
+
+ }
+ }
+
+ return resultFraction;
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexHull.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexHull.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexHull.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexHull.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1174 @@
+/*
+Stan Melax Convex Hull Computation
+Copyright (c) 2003-2006 Stan Melax http://www.melax.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 <string.h>
+
+#include "LinearMath/btConvexHull.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btVector3.h"
+
+
+
+template <class T>
+void Swap(T &a,T &b)
+{
+ T tmp = a;
+ a=b;
+ b=tmp;
+}
+
+
+//----------------------------------
+
+class int3
+{
+public:
+ int x,y,z;
+ int3(){};
+ int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
+ const int& operator[](int i) const {return (&x)[i];}
+ int& operator[](int i) {return (&x)[i];}
+};
+
+
+//------- btPlane ----------
+
+
+inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);}
+inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
+inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); }
+
+
+//--------- Utility Functions ------
+
+btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
+btVector3 PlaneProject(const btPlane &plane, const btVector3 &point);
+
+btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2);
+btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2)
+{
+ btVector3 N1 = p0.normal;
+ btVector3 N2 = p1.normal;
+ btVector3 N3 = p2.normal;
+
+ btVector3 n2n3; n2n3 = N2.cross(N3);
+ btVector3 n3n1; n3n1 = N3.cross(N1);
+ btVector3 n1n2; n1n2 = N1.cross(N2);
+
+ btScalar quotient = (N1.dot(n2n3));
+
+ btAssert(btFabs(quotient) > btScalar(0.000001));
+
+ quotient = btScalar(-1.) / quotient;
+ n2n3 *= p0.dist;
+ n3n1 *= p1.dist;
+ n1n2 *= p2.dist;
+ btVector3 potentialVertex = n2n3;
+ potentialVertex += n3n1;
+ potentialVertex += n1n2;
+ potentialVertex *= quotient;
+
+ btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ());
+ return result;
+
+}
+
+btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL);
+btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
+btVector3 NormalOf(const btVector3 *vert, const int n);
+
+
+btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1)
+{
+ // returns the point where the line p0-p1 intersects the plane n&d
+ static btVector3 dif;
+ dif = p1-p0;
+ btScalar dn= btDot(plane.normal,dif);
+ btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn;
+ return p0 + (dif*t);
+}
+
+btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
+{
+ return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
+}
+
+btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
+{
+ // return the normal of the triangle
+ // inscribed by v0, v1, and v2
+ btVector3 cp=btCross(v1-v0,v2-v1);
+ btScalar m=cp.length();
+ if(m==0) return btVector3(1,0,0);
+ return cp*(btScalar(1.0)/m);
+}
+
+
+btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
+{
+ static btVector3 cp;
+ cp = btCross(udir,vdir).normalized();
+
+ btScalar distu = -btDot(cp,ustart);
+ btScalar distv = -btDot(cp,vstart);
+ btScalar dist = (btScalar)fabs(distu-distv);
+ if(upoint)
+ {
+ btPlane plane;
+ plane.normal = btCross(vdir,cp).normalized();
+ plane.dist = -btDot(plane.normal,vstart);
+ *upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
+ }
+ if(vpoint)
+ {
+ btPlane plane;
+ plane.normal = btCross(udir,cp).normalized();
+ plane.dist = -btDot(plane.normal,ustart);
+ *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
+ }
+ return dist;
+}
+
+
+
+
+
+
+
+#define COPLANAR (0)
+#define UNDER (1)
+#define OVER (2)
+#define SPLIT (OVER|UNDER)
+#define PAPERWIDTH (btScalar(0.001))
+
+btScalar planetestepsilon = PAPERWIDTH;
+
+
+
+typedef ConvexH::HalfEdge HalfEdge;
+
+ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
+{
+ vertices.resize(vertices_size);
+ edges.resize(edges_size);
+ facets.resize(facets_size);
+}
+
+
+int PlaneTest(const btPlane &p, const btVector3 &v);
+int PlaneTest(const btPlane &p, const btVector3 &v) {
+ btScalar a = btDot(v,p.normal)+p.dist;
+ int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
+ return flag;
+}
+
+int SplitTest(ConvexH &convex,const btPlane &plane);
+int SplitTest(ConvexH &convex,const btPlane &plane) {
+ int flag=0;
+ for(int i=0;i<convex.vertices.size();i++) {
+ flag |= PlaneTest(plane,convex.vertices[i]);
+ }
+ return flag;
+}
+
+class VertFlag
+{
+public:
+ unsigned char planetest;
+ unsigned char junk;
+ unsigned char undermap;
+ unsigned char overmap;
+};
+class EdgeFlag
+{
+public:
+ unsigned char planetest;
+ unsigned char fixes;
+ short undermap;
+ short overmap;
+};
+class PlaneFlag
+{
+public:
+ unsigned char undermap;
+ unsigned char overmap;
+};
+class Coplanar{
+public:
+ unsigned short ea;
+ unsigned char v0;
+ unsigned char v1;
+};
+
+
+
+
+
+
+
+
+template<class T>
+int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
+{
+ btAssert(count);
+ int m=-1;
+ for(int i=0;i<count;i++)
+ if(allow[i])
+ {
+ if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir))
+ m=i;
+ }
+ btAssert(m!=-1);
+ return m;
+}
+
+btVector3 orth(const btVector3 &v);
+btVector3 orth(const btVector3 &v)
+{
+ btVector3 a=btCross(v,btVector3(0,0,1));
+ btVector3 b=btCross(v,btVector3(0,1,0));
+ if (a.length() > b.length())
+ {
+ return a.normalized();
+ } else {
+ return b.normalized();
+ }
+}
+
+
+template<class T>
+int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
+{
+ int m=-1;
+ while(m==-1)
+ {
+ m = maxdirfiltered(p,count,dir,allow);
+ if(allow[m]==3) return m;
+ T u = orth(dir);
+ T v = btCross(u,dir);
+ int ma=-1;
+ for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
+ {
+ btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
+ btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
+ int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
+ if(ma==m && mb==m)
+ {
+ allow[m]=3;
+ return m;
+ }
+ if(ma!=-1 && ma!=mb) // Yuck - this is really ugly
+ {
+ int mc = ma;
+ for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
+ {
+ btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
+ btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
+ int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
+ if(mc==m && md==m)
+ {
+ allow[m]=3;
+ return m;
+ }
+ mc=md;
+ }
+ }
+ ma=mb;
+ }
+ allow[m]=0;
+ m=-1;
+ }
+ btAssert(0);
+ return m;
+}
+
+
+
+
+int operator ==(const int3 &a,const int3 &b);
+int operator ==(const int3 &a,const int3 &b)
+{
+ for(int i=0;i<3;i++)
+ {
+ if(a[i]!=b[i]) return 0;
+ }
+ return 1;
+}
+
+
+int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon);
+int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon)
+{
+ btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
+ return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
+}
+int hasedge(const int3 &t, int a,int b);
+int hasedge(const int3 &t, int a,int b)
+{
+ for(int i=0;i<3;i++)
+ {
+ int i1= (i+1)%3;
+ if(t[i]==a && t[i1]==b) return 1;
+ }
+ return 0;
+}
+int hasvert(const int3 &t, int v);
+int hasvert(const int3 &t, int v)
+{
+ return (t[0]==v || t[1]==v || t[2]==v) ;
+}
+int shareedge(const int3 &a,const int3 &b);
+int shareedge(const int3 &a,const int3 &b)
+{
+ int i;
+ for(i=0;i<3;i++)
+ {
+ int i1= (i+1)%3;
+ if(hasedge(a,b[i1],b[i])) return 1;
+ }
+ return 0;
+}
+
+class btHullTriangle;
+
+
+
+class btHullTriangle : public int3
+{
+public:
+ int3 n;
+ int id;
+ int vmax;
+ btScalar rise;
+ btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
+ {
+ vmax=-1;
+ rise = btScalar(0.0);
+ }
+ ~btHullTriangle()
+ {
+ }
+ int &neib(int a,int b);
+};
+
+
+int &btHullTriangle::neib(int a,int b)
+{
+ static int er=-1;
+ int i;
+ for(i=0;i<3;i++)
+ {
+ int i1=(i+1)%3;
+ int i2=(i+2)%3;
+ if((*this)[i]==a && (*this)[i1]==b) return n[i2];
+ if((*this)[i]==b && (*this)[i1]==a) return n[i2];
+ }
+ btAssert(0);
+ return er;
+}
+void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t)
+{
+ int i;
+ for(i=0;i<3;i++)
+ {
+ int i1=(i+1)%3;
+ int i2=(i+2)%3;
+ int a = (*s)[i1];
+ int b = (*s)[i2];
+ btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id);
+ btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id);
+ m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
+ m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
+ }
+}
+
+void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t)
+{
+ b2bfix(s,t);
+ deAllocateTriangle(s);
+
+ deAllocateTriangle(t);
+}
+
+void HullLibrary::checkit(btHullTriangle *t)
+{
+ (void)t;
+
+ int i;
+ btAssert(m_tris[t->id]==t);
+ for(i=0;i<3;i++)
+ {
+ int i1=(i+1)%3;
+ int i2=(i+2)%3;
+ int a = (*t)[i1];
+ int b = (*t)[i2];
+
+ // release compile fix
+ (void)i1;
+ (void)i2;
+ (void)a;
+ (void)b;
+
+ btAssert(a!=b);
+ btAssert( m_tris[t->n[i]]->neib(b,a) == t->id);
+ }
+}
+
+btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c)
+{
+ void* mem = btAlignedAlloc(sizeof(btHullTriangle),16);
+ btHullTriangle* tr = new (mem)btHullTriangle(a,b,c);
+ tr->id = m_tris.size();
+ m_tris.push_back(tr);
+
+ return tr;
+}
+
+void HullLibrary::deAllocateTriangle(btHullTriangle* tri)
+{
+ btAssert(m_tris[tri->id]==tri);
+ m_tris[tri->id]=NULL;
+ tri->~btHullTriangle();
+ btAlignedFree(tri);
+}
+
+
+void HullLibrary::extrude(btHullTriangle *t0,int v)
+{
+ int3 t= *t0;
+ int n = m_tris.size();
+ 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;
+ 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;
+ 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);
+ checkit(tb);
+ checkit(tc);
+ if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]);
+ if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]);
+ if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]);
+ deAllocateTriangle(t0);
+
+}
+
+btHullTriangle* HullLibrary::extrudable(btScalar epsilon)
+{
+ int i;
+ btHullTriangle *t=NULL;
+ for(i=0;i<m_tris.size();i++)
+ {
+ if(!t || (m_tris[i] && t->rise<m_tris[i]->rise))
+ {
+ t = m_tris[i];
+ }
+ }
+ return (t->rise >epsilon)?t:NULL ;
+}
+
+
+
+
+int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow)
+{
+ btVector3 basis[3];
+ basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) );
+ int p0 = maxdirsterid(verts,verts_count, basis[0],allow);
+ int p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
+ basis[0] = verts[p0]-verts[p1];
+ if(p0==p1 || basis[0]==btVector3(0,0,0))
+ return int4(-1,-1,-1,-1);
+ basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
+ basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]);
+ if (basis[1].length() > basis[2].length())
+ {
+ basis[1].normalize();
+ } else {
+ basis[1] = basis[2];
+ basis[1].normalize ();
+ }
+ int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
+ if(p2 == p0 || p2 == p1)
+ {
+ p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
+ }
+ if(p2 == p0 || p2 == p1)
+ return int4(-1,-1,-1,-1);
+ basis[1] = verts[p2] - verts[p0];
+ basis[2] = btCross(basis[1],basis[0]).normalized();
+ int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
+ if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
+ if(p3==p0||p3==p1||p3==p2)
+ return int4(-1,-1,-1,-1);
+ btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
+ if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
+ return int4(p0,p1,p2,p3);
+}
+
+int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
+{
+ if(verts_count <4) return 0;
+ if(vlimit==0) vlimit=1000000000;
+ int j;
+ btVector3 bmin(*verts),bmax(*verts);
+ btAlignedObjectArray<int> isextreme;
+ isextreme.reserve(verts_count);
+ btAlignedObjectArray<int> allow;
+ allow.reserve(verts_count);
+
+ for(j=0;j<verts_count;j++)
+ {
+ allow.push_back(1);
+ isextreme.push_back(0);
+ bmin.setMin (verts[j]);
+ bmax.setMax (verts[j]);
+ }
+ btScalar epsilon = (bmax-bmin).length() * btScalar(0.001);
+ btAssert (epsilon != 0.0);
+
+
+ int4 p = FindSimplex(verts,verts_count,allow);
+ if(p.x==-1) return 0; // simplex failed
+
+
+
+ btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point
+ 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;j<m_tris.size();j++)
+ {
+ btHullTriangle *t=m_tris[j];
+ btAssert(t);
+ btAssert(t->vmax<0);
+ btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
+ t->vmax = maxdirsterid(verts,verts_count,n,allow);
+ t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
+ }
+ btHullTriangle *te;
+ vlimit-=4;
+ while(vlimit >0 && ((te=extrudable(epsilon)) != 0))
+ {
+ int3 ti=*te;
+ int v=te->vmax;
+ btAssert(v != -1);
+ btAssert(!isextreme[v]); // wtf we've already done this vertex
+ isextreme[v]=1;
+ //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
+ j=m_tris.size();
+ while(j--) {
+ if(!m_tris[j]) continue;
+ int3 t=*m_tris[j];
+ if(above(verts,t,verts[v],btScalar(0.01)*epsilon))
+ {
+ extrude(m_tris[j],v);
+ }
+ }
+ // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
+ j=m_tris.size();
+ while(j--)
+ {
+ if(!m_tris[j]) continue;
+ if(!hasvert(*m_tris[j],v)) break;
+ int3 nt=*m_tris[j];
+ if(above(verts,nt,center,btScalar(0.01)*epsilon) || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
+ {
+ btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
+ btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j);
+ extrude(nb,v);
+ j=m_tris.size();
+ }
+ }
+ j=m_tris.size();
+ while(j--)
+ {
+ btHullTriangle *t=m_tris[j];
+ if(!t) continue;
+ if(t->vmax>=0) break;
+ btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
+ t->vmax = maxdirsterid(verts,verts_count,n,allow);
+ if(isextreme[t->vmax])
+ {
+ t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate.
+ }
+ else
+ {
+ t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
+ }
+ }
+ vlimit --;
+ }
+ return 1;
+}
+
+int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit)
+{
+ int rc=calchullgen(verts,verts_count, vlimit) ;
+ if(!rc) return 0;
+ btAlignedObjectArray<int> ts;
+ int i;
+
+ for(i=0;i<m_tris.size();i++)
+ {
+ if(m_tris[i])
+ {
+ for(int j=0;j<3;j++)
+ ts.push_back((*m_tris[i])[j]);
+ deAllocateTriangle(m_tris[i]);
+ }
+ }
+ tris_count = ts.size()/3;
+ tris_out.resize(ts.size());
+
+ for (i=0;i<ts.size();i++)
+ {
+ tris_out[i] = static_cast<unsigned int>(ts[i]);
+ }
+ m_tris.resize(0);
+
+ return 1;
+}
+
+
+
+
+
+bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit)
+{
+
+ int tris_count;
+ int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) );
+ if(!ret) return false;
+ result.mIndexCount = (unsigned int) (tris_count*3);
+ result.mFaceCount = (unsigned int) tris_count;
+ result.mVertices = (btVector3*) vertices;
+ result.mVcount = (unsigned int) vcount;
+ return true;
+
+}
+
+
+void ReleaseHull(PHullResult &result);
+void ReleaseHull(PHullResult &result)
+{
+ if ( result.m_Indices.size() )
+ {
+ result.m_Indices.clear();
+ }
+
+ result.mVcount = 0;
+ result.mIndexCount = 0;
+ result.mVertices = 0;
+}
+
+
+//*********************************************************************
+//*********************************************************************
+//******** HullLib header
+//*********************************************************************
+//*********************************************************************
+
+//*********************************************************************
+//*********************************************************************
+//******** HullLib implementation
+//*********************************************************************
+//*********************************************************************
+
+HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request
+ HullResult &result) // contains the resulst
+{
+ HullError ret = QE_FAIL;
+
+
+ PHullResult hr;
+
+ unsigned int vcount = desc.mVcount;
+ if ( vcount < 8 ) vcount = 8;
+
+ btAlignedObjectArray<btVector3> vertexSource;
+ vertexSource.resize(static_cast<int>(vcount));
+
+ btVector3 scale;
+
+ unsigned int ovcount;
+
+ bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates!
+
+ if ( ok )
+ {
+
+
+// if ( 1 ) // scale vertices back to their original size.
+ {
+ for (unsigned int i=0; i<ovcount; i++)
+ {
+ btVector3& v = vertexSource[static_cast<int>(i)];
+ v[0]*=scale[0];
+ v[1]*=scale[1];
+ v[2]*=scale[2];
+ }
+ }
+
+ ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices);
+
+ if ( ok )
+ {
+
+ // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
+ btAlignedObjectArray<btVector3> vertexScratch;
+ vertexScratch.resize(static_cast<int>(hr.mVcount));
+
+ BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount );
+
+ ret = QE_OK;
+
+ if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle!
+ {
+ result.mPolygons = false;
+ result.mNumOutputVertices = ovcount;
+ result.m_OutputVertices.resize(static_cast<int>(ovcount));
+ result.mNumFaces = hr.mFaceCount;
+ result.mNumIndices = hr.mIndexCount;
+
+ result.m_Indices.resize(static_cast<int>(hr.mIndexCount));
+
+ memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
+
+ if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
+ {
+
+ const unsigned int *source = &hr.m_Indices[0];
+ unsigned int *dest = &result.m_Indices[0];
+
+ for (unsigned int i=0; i<hr.mFaceCount; i++)
+ {
+ dest[0] = source[2];
+ dest[1] = source[1];
+ dest[2] = source[0];
+ dest+=3;
+ source+=3;
+ }
+
+ }
+ else
+ {
+ memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount);
+ }
+ }
+ else
+ {
+ result.mPolygons = true;
+ result.mNumOutputVertices = ovcount;
+ result.m_OutputVertices.resize(static_cast<int>(ovcount));
+ result.mNumFaces = hr.mFaceCount;
+ result.mNumIndices = hr.mIndexCount+hr.mFaceCount;
+ result.m_Indices.resize(static_cast<int>(result.mNumIndices));
+ memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
+
+// if ( 1 )
+ {
+ const unsigned int *source = &hr.m_Indices[0];
+ unsigned int *dest = &result.m_Indices[0];
+ for (unsigned int i=0; i<hr.mFaceCount; i++)
+ {
+ dest[0] = 3;
+ if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
+ {
+ dest[1] = source[2];
+ dest[2] = source[1];
+ dest[3] = source[0];
+ }
+ else
+ {
+ dest[1] = source[0];
+ dest[2] = source[1];
+ dest[3] = source[2];
+ }
+
+ dest+=4;
+ source+=3;
+ }
+ }
+ }
+ ReleaseHull(hr);
+ }
+ }
+
+ return ret;
+}
+
+
+
+HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it.
+{
+ if ( result.m_OutputVertices.size())
+ {
+ result.mNumOutputVertices=0;
+ result.m_OutputVertices.clear();
+ }
+ if ( result.m_Indices.size() )
+ {
+ result.mNumIndices=0;
+ result.m_Indices.clear();
+ }
+ return QE_OK;
+}
+
+
+static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z)
+{
+ // XXX, might be broken
+ btVector3& dest = p[vcount];
+ dest[0] = x;
+ dest[1] = y;
+ dest[2] = z;
+ vcount++;
+}
+
+btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2);
+btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2)
+{
+
+ btScalar dx = px - p2[0];
+ btScalar dy = py - p2[1];
+ btScalar dz = pz - p2[2];
+
+ return dx*dx+dy*dy+dz*dz;
+}
+
+
+
+bool HullLibrary::CleanupVertices(unsigned int svcount,
+ const btVector3 *svertices,
+ unsigned int stride,
+ unsigned int &vcount, // output number of vertices
+ btVector3 *vertices, // location to store the results.
+ btScalar normalepsilon,
+ btVector3& scale)
+{
+ if ( svcount == 0 ) return false;
+
+ m_vertexIndexMapping.resize(0);
+
+
+#define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */
+
+ vcount = 0;
+
+ btScalar recip[3]={0.f,0.f,0.f};
+
+ if ( scale )
+ {
+ scale[0] = 1;
+ scale[1] = 1;
+ scale[2] = 1;
+ }
+
+ btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
+ btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
+
+ const char *vtx = (const char *) svertices;
+
+// if ( 1 )
+ {
+ for (unsigned int i=0; i<svcount; i++)
+ {
+ const btScalar *p = (const btScalar *) vtx;
+
+ vtx+=stride;
+
+ for (int j=0; j<3; j++)
+ {
+ if ( p[j] < bmin[j] ) bmin[j] = p[j];
+ if ( p[j] > bmax[j] ) bmax[j] = p[j];
+ }
+ }
+ }
+
+ btScalar dx = bmax[0] - bmin[0];
+ btScalar dy = bmax[1] - bmin[1];
+ btScalar dz = bmax[2] - bmin[2];
+
+ btVector3 center;
+
+ center[0] = dx*btScalar(0.5) + bmin[0];
+ center[1] = dy*btScalar(0.5) + bmin[1];
+ center[2] = dz*btScalar(0.5) + bmin[2];
+
+ if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
+ {
+
+ btScalar len = FLT_MAX;
+
+ if ( dx > EPSILON && dx < len ) len = dx;
+ if ( dy > EPSILON && dy < len ) len = dy;
+ if ( dz > EPSILON && dz < len ) len = dz;
+
+ if ( len == FLT_MAX )
+ {
+ dx = dy = dz = btScalar(0.01); // one centimeter
+ }
+ else
+ {
+ if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
+ if ( dy < EPSILON ) dy = len * btScalar(0.05);
+ if ( dz < EPSILON ) dz = len * btScalar(0.05);
+ }
+
+ btScalar x1 = center[0] - dx;
+ btScalar x2 = center[0] + dx;
+
+ btScalar y1 = center[1] - dy;
+ btScalar y2 = center[1] + dy;
+
+ btScalar z1 = center[2] - dz;
+ btScalar z2 = center[2] + dz;
+
+ addPoint(vcount,vertices,x1,y1,z1);
+ addPoint(vcount,vertices,x2,y1,z1);
+ addPoint(vcount,vertices,x2,y2,z1);
+ addPoint(vcount,vertices,x1,y2,z1);
+ addPoint(vcount,vertices,x1,y1,z2);
+ addPoint(vcount,vertices,x2,y1,z2);
+ addPoint(vcount,vertices,x2,y2,z2);
+ addPoint(vcount,vertices,x1,y2,z2);
+
+ return true; // return cube
+
+
+ }
+ else
+ {
+ if ( scale )
+ {
+ scale[0] = dx;
+ scale[1] = dy;
+ scale[2] = dz;
+
+ recip[0] = 1 / dx;
+ recip[1] = 1 / dy;
+ recip[2] = 1 / dz;
+
+ center[0]*=recip[0];
+ center[1]*=recip[1];
+ center[2]*=recip[2];
+
+ }
+
+ }
+
+
+
+ vtx = (const char *) svertices;
+
+ for (unsigned int i=0; i<svcount; i++)
+ {
+ const btVector3 *p = (const btVector3 *)vtx;
+ vtx+=stride;
+
+ btScalar px = p->getX();
+ btScalar py = p->getY();
+ btScalar pz = p->getZ();
+
+ if ( scale )
+ {
+ px = px*recip[0]; // normalize
+ py = py*recip[1]; // normalize
+ pz = pz*recip[2]; // normalize
+ }
+
+// if ( 1 )
+ {
+ unsigned int j;
+
+ for (j=0; j<vcount; j++)
+ {
+ /// XXX might be broken
+ btVector3& v = vertices[j];
+
+ btScalar x = v[0];
+ btScalar y = v[1];
+ btScalar z = v[2];
+
+ btScalar dx = fabsf(x - px );
+ btScalar dy = fabsf(y - py );
+ btScalar dz = fabsf(z - pz );
+
+ if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
+ {
+ // ok, it is close enough to the old one
+ // now let us see if it is further from the center of the point cloud than the one we already recorded.
+ // in which case we keep this one instead.
+
+ btScalar dist1 = GetDist(px,py,pz,center);
+ btScalar dist2 = GetDist(v[0],v[1],v[2],center);
+
+ if ( dist1 > dist2 )
+ {
+ v[0] = px;
+ v[1] = py;
+ v[2] = pz;
+
+ }
+
+ break;
+ }
+ }
+
+ if ( j == vcount )
+ {
+ btVector3& dest = vertices[vcount];
+ dest[0] = px;
+ dest[1] = py;
+ dest[2] = pz;
+ vcount++;
+ }
+ m_vertexIndexMapping.push_back(j);
+ }
+ }
+
+ // ok..now make sure we didn't prune so many vertices it is now invalid.
+// if ( 1 )
+ {
+ btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
+ btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
+
+ for (unsigned int i=0; i<vcount; i++)
+ {
+ const btVector3& p = vertices[i];
+ for (int j=0; j<3; j++)
+ {
+ if ( p[j] < bmin[j] ) bmin[j] = p[j];
+ if ( p[j] > bmax[j] ) bmax[j] = p[j];
+ }
+ }
+
+ btScalar dx = bmax[0] - bmin[0];
+ btScalar dy = bmax[1] - bmin[1];
+ btScalar dz = bmax[2] - bmin[2];
+
+ if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
+ {
+ btScalar cx = dx*btScalar(0.5) + bmin[0];
+ btScalar cy = dy*btScalar(0.5) + bmin[1];
+ btScalar cz = dz*btScalar(0.5) + bmin[2];
+
+ btScalar len = FLT_MAX;
+
+ if ( dx >= EPSILON && dx < len ) len = dx;
+ if ( dy >= EPSILON && dy < len ) len = dy;
+ if ( dz >= EPSILON && dz < len ) len = dz;
+
+ if ( len == FLT_MAX )
+ {
+ dx = dy = dz = btScalar(0.01); // one centimeter
+ }
+ else
+ {
+ if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
+ if ( dy < EPSILON ) dy = len * btScalar(0.05);
+ if ( dz < EPSILON ) dz = len * btScalar(0.05);
+ }
+
+ btScalar x1 = cx - dx;
+ btScalar x2 = cx + dx;
+
+ btScalar y1 = cy - dy;
+ btScalar y2 = cy + dy;
+
+ btScalar z1 = cz - dz;
+ btScalar z2 = cz + dz;
+
+ vcount = 0; // add box
+
+ addPoint(vcount,vertices,x1,y1,z1);
+ addPoint(vcount,vertices,x2,y1,z1);
+ addPoint(vcount,vertices,x2,y2,z1);
+ addPoint(vcount,vertices,x1,y2,z1);
+ addPoint(vcount,vertices,x1,y1,z2);
+ addPoint(vcount,vertices,x2,y1,z2);
+ addPoint(vcount,vertices,x2,y2,z2);
+ addPoint(vcount,vertices,x1,y2,z2);
+
+ return true;
+ }
+ }
+
+ return true;
+}
+
+void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
+{
+ btAlignedObjectArray<int>tmpIndices;
+ tmpIndices.resize(m_vertexIndexMapping.size());
+ int i;
+
+ for (i=0;i<m_vertexIndexMapping.size();i++)
+ {
+ tmpIndices[i] = m_vertexIndexMapping[i];
+ }
+
+ TUIntArray usedIndices;
+ usedIndices.resize(static_cast<int>(vcount));
+ memset(&usedIndices[0],0,sizeof(unsigned int)*vcount);
+
+ ocount = 0;
+
+ for (i=0; i<int (indexcount); i++)
+ {
+ unsigned int v = indices[i]; // original array index
+
+ btAssert( v >= 0 && v < vcount );
+
+ if ( usedIndices[static_cast<int>(v)] ) // if already remapped
+ {
+ indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array
+ }
+ else
+ {
+
+ indices[i] = ocount; // new index mapping
+
+ overts[ocount][0] = verts[v][0]; // copy old vert to new vert array
+ overts[ocount][1] = verts[v][1];
+ overts[ocount][2] = verts[v][2];
+
+ for (int k=0;k<m_vertexIndexMapping.size();k++)
+ {
+ if (tmpIndices[k]==v)
+ m_vertexIndexMapping[k]=ocount;
+ }
+
+ ocount++; // increment output vert count
+
+ btAssert( ocount >=0 && ocount <= vcount );
+
+ usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping
+
+
+ }
+ }
+
+
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexHullShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexHullShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexHullShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexHullShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,188 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+#include "LinearMath/btQuaternion.h"
+
+
+btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape ()
+{
+ m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE;
+ m_unscaledPoints.resize(numPoints);
+
+ unsigned char* pointsAddress = (unsigned char*)points;
+
+ for (int i=0;i<numPoints;i++)
+ {
+ btScalar* point = (btScalar*)pointsAddress;
+ m_unscaledPoints[i] = btVector3(point[0], point[1], point[2]);
+ pointsAddress += stride;
+ }
+
+ recalcLocalAabb();
+
+}
+
+
+
+void btConvexHullShape::setLocalScaling(const btVector3& scaling)
+{
+ m_localScaling = scaling;
+ recalcLocalAabb();
+}
+
+void btConvexHullShape::addPoint(const btVector3& point)
+{
+ m_unscaledPoints.push_back(point);
+ recalcLocalAabb();
+
+}
+
+btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+ btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+ btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
+
+ 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<m_unscaledPoints.size();i++)
+ {
+ btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
+
+ newDot = vec.dot(vtx);
+ if (newDot > maxDot)
+ {
+ maxDot = newDot;
+ supVec = vtx;
+ }
+ }
+ return supVec;
+}
+
+void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+ btScalar newDot;
+ //use 'w' component of supportVerticesOut?
+ {
+ for (int i=0;i<numVectors;i++)
+ {
+ supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
+ }
+ }
+ for (int i=0;i<m_unscaledPoints.size();i++)
+ {
+ btVector3 vtx = getScaledPoint(i);
+
+ for (int j=0;j<numVectors;j++)
+ {
+ const btVector3& vec = vectors[j];
+
+ newDot = vec.dot(vtx);
+ if (newDot > supportVerticesOut[j][3])
+ {
+ //WARNING: don't swap next lines, the w component would get overwritten!
+ supportVerticesOut[j] = vtx;
+ supportVerticesOut[j][3] = newDot;
+ }
+ }
+ }
+
+
+
+}
+
+
+
+btVector3 btConvexHullShape::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;
+}
+
+
+
+
+
+
+
+
+
+//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 btConvexHullShape::getNumVertices() const
+{
+ return m_unscaledPoints.size();
+}
+
+int btConvexHullShape::getNumEdges() const
+{
+ return m_unscaledPoints.size();
+}
+
+void btConvexHullShape::getEdge(int i,btVector3& pa,btVector3& pb) const
+{
+
+ int index0 = i%m_unscaledPoints.size();
+ int index1 = (i+1)%m_unscaledPoints.size();
+ pa = getScaledPoint(index0);
+ pb = getScaledPoint(index1);
+}
+
+void btConvexHullShape::getVertex(int i,btVector3& vtx) const
+{
+ vtx = getScaledPoint(i);
+}
+
+int btConvexHullShape::getNumPlanes() const
+{
+ return 0;
+}
+
+void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const
+{
+
+ btAssert(0);
+}
+
+//not yet
+bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const
+{
+ btAssert(0);
+ return false;
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexInternalShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexInternalShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexInternalShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexInternalShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,151 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+
+
+
+btConvexInternalShape::btConvexInternalShape()
+: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
+m_collisionMargin(CONVEX_DISTANCE_MARGIN)
+{
+}
+
+
+void btConvexInternalShape::setLocalScaling(const btVector3& scaling)
+{
+ m_localScaling = scaling.absolute();
+}
+
+
+
+void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const
+{
+#ifndef __SPU__
+ //use localGetSupportingVertexWithoutMargin?
+ btScalar margin = getMargin();
+ for (int i=0;i<3;i++)
+ {
+ btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+ vec[i] = btScalar(1.);
+
+ btVector3 sv = localGetSupportingVertex(vec*trans.getBasis());
+
+ btVector3 tmp = trans(sv);
+ maxAabb[i] = tmp[i]+margin;
+ vec[i] = btScalar(-1.);
+ tmp = trans(localGetSupportingVertex(vec*trans.getBasis()));
+ minAabb[i] = tmp[i]-margin;
+ }
+#endif
+}
+
+
+
+btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const
+{
+#ifndef __SPU__
+
+ 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;
+
+#else
+ btAssert(0);
+ return btVector3(0,0,0);
+#endif //__SPU__
+
+ }
+
+
+btConvexInternalAabbCachingShape::btConvexInternalAabbCachingShape()
+ : btConvexInternalShape(),
+m_localAabbMin(1,1,1),
+m_localAabbMax(-1,-1,-1),
+m_isLocalAabbValid(false)
+{
+}
+
+
+void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
+}
+
+void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling)
+{
+ btConvexInternalShape::setLocalScaling(scaling);
+ recalcLocalAabb();
+}
+
+
+void btConvexInternalAabbCachingShape::recalcLocalAabb()
+{
+ m_isLocalAabbValid = true;
+
+ #if 1
+ static const btVector3 _directions[] =
+ {
+ btVector3( 1., 0., 0.),
+ btVector3( 0., 1., 0.),
+ btVector3( 0., 0., 1.),
+ btVector3( -1., 0., 0.),
+ btVector3( 0., -1., 0.),
+ btVector3( 0., 0., -1.)
+ };
+
+ btVector3 _supporting[] =
+ {
+ btVector3( 0., 0., 0.),
+ btVector3( 0., 0., 0.),
+ btVector3( 0., 0., 0.),
+ btVector3( 0., 0., 0.),
+ btVector3( 0., 0., 0.),
+ btVector3( 0., 0., 0.)
+ };
+
+ batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
+
+ for ( int i = 0; i < 3; ++i )
+ {
+ m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
+ m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
+ }
+
+ #else
+
+ for (int i=0;i<3;i++)
+ {
+ btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+ vec[i] = btScalar(1.);
+ btVector3 tmp = localGetSupportingVertex(vec);
+ m_localAabbMax[i] = tmp[i]+m_collisionMargin;
+ vec[i] = btScalar(-1.);
+ tmp = localGetSupportingVertex(vec);
+ m_localAabbMin[i] = tmp[i]-m_collisionMargin;
+ }
+ #endif
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexPlaneCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexPlaneCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexPlaneCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexPlaneCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,155 @@
+/*
+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 "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h"
+
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+
+//#include <stdio.h>
+
+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_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);
+ m_ownManifold = true;
+ }
+}
+
+
+btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
+{
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr)
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ }
+}
+
+void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ 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();
+
+ btTransform convexWorldTransform = convexObj->getWorldTransform();
+ btTransform convexInPlaneTrans;
+ 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);
+
+ btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
+ btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected;
+
+ hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
+ resultOut->setPersistentManifold(m_manifoldPtr);
+ if (hasCollision)
+ {
+ /// report a contact. internally this will be kept persistent, and contact reduction is done
+ btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal;
+ 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();
+
+
+ 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()<m_minimumPointsPerturbationThreshold)
+ {
+ btVector3 v0,v1;
+ btPlaneSpace1(planeNormal,v0,v1);
+ //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
+
+ const btScalar angleLimit = 0.125f * SIMD_PI;
+ btScalar perturbeAngle;
+ btScalar radius = convexShape->getAngularMotionDisc();
+ perturbeAngle = gContactBreakingThreshold / radius;
+ if ( perturbeAngle > angleLimit )
+ perturbeAngle = angleLimit;
+
+ btQuaternion perturbeRot(v0,perturbeAngle);
+ for (int i=0;i<m_numPerturbationIterations;i++)
+ {
+ btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
+ btQuaternion rotq(planeNormal,iterationAngle);
+ collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0,body1,dispatchInfo,resultOut);
+ }
+ }
+
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr->getNumContacts())
+ {
+ resultOut->refreshContactPoints();
+ }
+ }
+}
+
+btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)resultOut;
+ (void)dispatchInfo;
+ (void)col0;
+ (void)col1;
+
+ //not yet
+ return btScalar(1.);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexPointCloudShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexPointCloudShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexPointCloudShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexPointCloudShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,157 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/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(-BT_LARGE_FLOAT);
+
+ 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<m_numPoints;i++)
+ {
+ btVector3 vtx = getScaledPoint(i);
+
+ newDot = vec.dot(vtx);
+ if (newDot > 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<numVectors;i++)
+ {
+ supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
+ }
+ }
+ for (int i=0;i<m_numPoints;i++)
+ {
+ btVector3 vtx = getScaledPoint(i);
+
+ for (int j=0;j<numVectors;j++)
+ {
+ const btVector3& vec = vectors[j];
+
+ newDot = vec.dot(vtx);
+ if (newDot > 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;
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,429 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btConvexPointCloudShape.h"
+
+///not supported on IBM SDK, until we fix the alignment of btVector3
+#if defined (__CELLOS_LV2__) && defined (__SPU__)
+#include <spu_intrinsics.h>
+static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 )
+{
+ vec_float4 result;
+ result = spu_mul( vec0, vec1 );
+ result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result );
+ return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result );
+}
+#endif //__SPU__
+
+btConvexShape::btConvexShape ()
+{
+}
+
+btConvexShape::~btConvexShape()
+{
+
+}
+
+
+
+static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling)
+{
+
+ btVector3 vec = localDirOrg * localScaling;
+
+#if defined (__CELLOS_LV2__) && defined (__SPU__)
+
+ btVector3 localDir = vec;
+
+ vec_float4 v_distMax = {-FLT_MAX,0,0,0};
+ vec_int4 v_idxMax = {-999,0,0,0};
+ int v=0;
+ int numverts = numPoints;
+
+ for(;v<(int)numverts-4;v+=4) {
+ vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128());
+ vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128());
+ vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128());
+ vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128());
+ const vec_int4 i0 = {v ,0,0,0};
+ const vec_int4 i1 = {v+1,0,0,0};
+ const vec_int4 i2 = {v+2,0,0,0};
+ const vec_int4 i3 = {v+3,0,0,0};
+ vec_uint4 retGt01 = spu_cmpgt(p0,p1);
+ vec_float4 pmax01 = spu_sel(p1,p0,retGt01);
+ vec_int4 imax01 = spu_sel(i1,i0,retGt01);
+ vec_uint4 retGt23 = spu_cmpgt(p2,p3);
+ vec_float4 pmax23 = spu_sel(p3,p2,retGt23);
+ vec_int4 imax23 = spu_sel(i3,i2,retGt23);
+ vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23);
+ vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123);
+ vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123);
+ vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123);
+ v_distMax = spu_sel(pmax0123,v_distMax,retGtMax);
+ v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax);
+ }
+ for(;v<(int)numverts;v++) {
+ vec_float4 p = vec_dot3(points[v].get128(),localDir.get128());
+ const vec_int4 i = {v,0,0,0};
+ vec_uint4 retGtMax = spu_cmpgt(v_distMax,p);
+ v_distMax = spu_sel(p,v_distMax,retGtMax);
+ v_idxMax = spu_sel(i,v_idxMax,retGtMax);
+ }
+ int ptIndex = spu_extract(v_idxMax,0);
+ const btVector3& supVec= points[ptIndex] * localScaling;
+ return supVec;
+#else
+
+ btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
+ int ptIndex = -1;
+
+ for (int i=0;i<numPoints;i++)
+ {
+
+ newDot = vec.dot(points[i]);
+ if (newDot > maxDot)
+ {
+ maxDot = newDot;
+ ptIndex = i;
+ }
+ }
+ btAssert(ptIndex >= 0);
+ btVector3 supVec = points[ptIndex] * localScaling;
+ return supVec;
+#endif //__SPU__
+}
+
+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(-BT_LARGE_FLOAT));
+
+ 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);
+}
+#ifndef __SPU__
+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:
+ {
+ btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)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);
+}
+
+#endif //__SPU__
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexTriangleMeshShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexTriangleMeshShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexTriangleMeshShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btConvexTriangleMeshShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,315 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+#include "LinearMath/btQuaternion.h"
+#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
+
+
+btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb)
+: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface)
+{
+ m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE;
+ if ( calcAabb )
+ recalcLocalAabb();
+}
+
+
+
+
+///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once
+///but then we are duplicating
+class LocalSupportVertexCallback: public btInternalTriangleIndexCallback
+{
+
+ btVector3 m_supportVertexLocal;
+public:
+
+ btScalar m_maxDot;
+ btVector3 m_supportVecLocal;
+
+ LocalSupportVertexCallback(const btVector3& supportVecLocal)
+ : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)),
+ m_maxDot(btScalar(-BT_LARGE_FLOAT)),
+ m_supportVecLocal(supportVecLocal)
+ {
+ }
+
+ virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
+ {
+ (void)triangleIndex;
+ (void)partId;
+
+ for (int i=0;i<3;i++)
+ {
+ btScalar dot = m_supportVecLocal.dot(triangle[i]);
+ if (dot > m_maxDot)
+ {
+ m_maxDot = dot;
+ m_supportVertexLocal = triangle[i];
+ }
+ }
+ }
+
+ btVector3 GetSupportVertexLocal()
+ {
+ return m_supportVertexLocal;
+ }
+
+};
+
+
+
+
+
+btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+ btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+
+ 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;
+ }
+
+ LocalSupportVertexCallback supportCallback(vec);
+ btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+ supVec = supportCallback.GetSupportVertexLocal();
+
+ return supVec;
+}
+
+void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+ //use 'w' component of supportVerticesOut?
+ {
+ for (int i=0;i<numVectors;i++)
+ {
+ supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
+ }
+ }
+
+ ///@todo: could do the batch inside the callback!
+
+
+ for (int j=0;j<numVectors;j++)
+ {
+ const btVector3& vec = vectors[j];
+ LocalSupportVertexCallback supportCallback(vec);
+ btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+ supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
+ }
+
+}
+
+
+
+btVector3 btConvexTriangleMeshShape::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;
+}
+
+
+
+
+
+
+
+
+
+//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
+//Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo
+int btConvexTriangleMeshShape::getNumVertices() const
+{
+ //cache this?
+ return 0;
+
+}
+
+int btConvexTriangleMeshShape::getNumEdges() const
+{
+ return 0;
+}
+
+void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const
+{
+ btAssert(0);
+}
+
+void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const
+{
+ btAssert(0);
+}
+
+int btConvexTriangleMeshShape::getNumPlanes() const
+{
+ return 0;
+}
+
+void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const
+{
+ btAssert(0);
+}
+
+//not yet
+bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const
+{
+ btAssert(0);
+ return false;
+}
+
+
+
+void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+{
+ m_stridingMesh->setScaling(scaling);
+
+ recalcLocalAabb();
+
+}
+
+
+const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
+{
+ return m_stridingMesh->getScaling();
+}
+
+void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
+{
+ class CenterCallback: public btInternalTriangleIndexCallback
+ {
+ bool first;
+ btVector3 ref;
+ btVector3 sum;
+ btScalar volume;
+
+ public:
+
+ CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
+ {
+ }
+
+ virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+ {
+ (void) triangleIndex;
+ (void) partId;
+ if (first)
+ {
+ ref = triangle[0];
+ first = false;
+ }
+ else
+ {
+ btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
+ sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
+ volume += vol;
+ }
+ }
+
+ btVector3 getCenter()
+ {
+ return (volume > 0) ? sum / volume : ref;
+ }
+
+ btScalar getVolume()
+ {
+ return volume * btScalar(1. / 6);
+ }
+
+ };
+
+ class InertiaCallback: public btInternalTriangleIndexCallback
+ {
+ btMatrix3x3 sum;
+ btVector3 center;
+
+ public:
+
+ InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
+ {
+ }
+
+ virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+ {
+ (void) triangleIndex;
+ (void) partId;
+ btMatrix3x3 i;
+ btVector3 a = triangle[0] - center;
+ btVector3 b = triangle[1] - center;
+ btVector3 c = triangle[2] - center;
+ btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
+ for (int j = 0; j < 3; j++)
+ {
+ for (int k = 0; k <= j; k++)
+ {
+ i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
+ + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
+ }
+ }
+ btScalar i00 = -i[0][0];
+ btScalar i11 = -i[1][1];
+ btScalar i22 = -i[2][2];
+ i[0][0] = i11 + i22;
+ i[1][1] = i22 + i00;
+ i[2][2] = i00 + i11;
+ sum[0] += i[0];
+ sum[1] += i[1];
+ sum[2] += i[2];
+ }
+
+ btMatrix3x3& getInertia()
+ {
+ return sum;
+ }
+
+ };
+
+ CenterCallback centerCallback;
+ btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax);
+ btVector3 center = centerCallback.getCenter();
+ principal.setOrigin(center);
+ volume = centerCallback.getVolume();
+
+ InertiaCallback inertiaCallback(center);
+ m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
+
+ btMatrix3x3& i = inertiaCallback.getInertia();
+ i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+ inertia.setValue(i[0][0], i[1][1], i[2][2]);
+ inertia /= volume;
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btCylinderShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btCylinderShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btCylinderShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btCylinderShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,222 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btCylinderShape.h"
+
+btCylinderShape::btCylinderShape (const btVector3& halfExtents)
+:btConvexInternalShape(),
+m_upAxis(1)
+{
+ btVector3 margin(getMargin(),getMargin(),getMargin());
+ m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin;
+ m_shapeType = CYLINDER_SHAPE_PROXYTYPE;
+}
+
+
+btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents)
+:btCylinderShape(halfExtents)
+{
+ m_upAxis = 0;
+
+}
+
+
+btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents)
+:btCylinderShape(halfExtents)
+{
+ m_upAxis = 2;
+
+}
+
+void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
+}
+
+void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ //approximation of box shape, todo: implement cylinder shape inertia before people notice ;-)
+ btVector3 halfExtents = getHalfExtentsWithMargin();
+
+ btScalar lx=btScalar(2.)*(halfExtents.x());
+ btScalar ly=btScalar(2.)*(halfExtents.y());
+ btScalar lz=btScalar(2.)*(halfExtents.z());
+
+ inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
+ mass/(btScalar(12.0)) * (lx*lx + lz*lz),
+ mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+
+}
+
+
+SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v)
+{
+const int cylinderUpAxis = 0;
+const int XX = 1;
+const int YY = 0;
+const int ZZ = 2;
+
+ //mapping depends on how cylinder local orientation is
+ // extents of the cylinder is: X,Y is for radius, and Z for height
+
+
+ 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 tmp;
+ }
+ else
+ {
+ tmp[XX] = radius;
+ tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+ tmp[ZZ] = btScalar(0.0);
+ return tmp;
+ }
+
+
+}
+
+
+
+
+
+
+inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v)
+{
+
+const int cylinderUpAxis = 1;
+const int XX = 0;
+const int YY = 1;
+const int ZZ = 2;
+
+
+ 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 tmp;
+ }
+ else
+ {
+ tmp[XX] = radius;
+ tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+ tmp[ZZ] = btScalar(0.0);
+ return tmp;
+ }
+
+}
+
+inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v)
+{
+const int cylinderUpAxis = 2;
+const int XX = 0;
+const int YY = 2;
+const int ZZ = 1;
+
+ //mapping depends on how cylinder local orientation is
+ // extents of the cylinder is: X,Y is for radius, and Z for height
+
+
+ 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 tmp;
+ }
+ else
+ {
+ tmp[XX] = radius;
+ tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+ tmp[ZZ] = btScalar(0.0);
+ return tmp;
+ }
+
+
+}
+
+btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+ return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec);
+}
+
+
+btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+ return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec);
+}
+btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+ return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec);
+}
+
+void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+ for (int i=0;i<numVectors;i++)
+ {
+ supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]);
+ }
+}
+
+void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+ for (int i=0;i<numVectors;i++)
+ {
+ supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]);
+ }
+}
+
+
+
+
+void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+ for (int i=0;i<numVectors;i++)
+ {
+ supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vectors[i]);
+ }
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btDbvt.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btDbvt.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btDbvt.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btDbvt.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1295 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btDbvt implementation by Nathanael Presson
+
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+
+//
+typedef btAlignedObjectArray<btDbvtNode*> tNodeArray;
+typedef btAlignedObjectArray<const btDbvtNode*> tConstNodeArray;
+
+//
+struct btDbvtNodeEnumerator : btDbvt::ICollide
+{
+ tConstNodeArray nodes;
+ void Process(const btDbvtNode* n) { nodes.push_back(n); }
+};
+
+//
+static DBVT_INLINE int indexof(const btDbvtNode* node)
+{
+ return(node->parent->childs[1]==node);
+}
+
+//
+static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a,
+ const btDbvtVolume& b)
+{
+#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)
+ ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]);
+ btDbvtVolume& res=*(btDbvtVolume*)locals;
+#else
+ btDbvtVolume res;
+#endif
+ Merge(a,b,res);
+ return(res);
+}
+
+// volume+edge lengths
+static DBVT_INLINE btScalar size(const btDbvtVolume& a)
+{
+ const btVector3 edges=a.Lengths();
+ return( edges.x()*edges.y()*edges.z()+
+ edges.x()+edges.y()+edges.z());
+}
+
+//
+static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth)
+{
+ if(node->isinternal())
+ {
+ getmaxdepth(node->childs[0],depth+1,maxdepth);
+ getmaxdepth(node->childs[0],depth+1,maxdepth);
+ } else maxdepth=btMax(maxdepth,depth);
+}
+
+//
+static DBVT_INLINE void deletenode( btDbvt* pdbvt,
+ btDbvtNode* node)
+{
+ btAlignedFree(pdbvt->m_free);
+ pdbvt->m_free=node;
+}
+
+//
+static void recursedeletenode( btDbvt* pdbvt,
+ btDbvtNode* node)
+{
+ if(!node->isleaf())
+ {
+ recursedeletenode(pdbvt,node->childs[0]);
+ recursedeletenode(pdbvt,node->childs[1]);
+ }
+ if(node==pdbvt->m_root) pdbvt->m_root=0;
+ deletenode(pdbvt,node);
+}
+
+//
+static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt,
+ btDbvtNode* parent,
+ void* data)
+{
+ btDbvtNode* node;
+ if(pdbvt->m_free)
+ { node=pdbvt->m_free;pdbvt->m_free=0; }
+ else
+ { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); }
+ node->parent = parent;
+ node->data = data;
+ node->childs[1] = 0;
+ return(node);
+}
+
+//
+static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt,
+ btDbvtNode* parent,
+ const btDbvtVolume& volume,
+ void* data)
+{
+ btDbvtNode* node=createnode(pdbvt,parent,data);
+ node->volume=volume;
+ return(node);
+}
+
+//
+static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt,
+ btDbvtNode* parent,
+ const btDbvtVolume& volume0,
+ const btDbvtVolume& volume1,
+ void* data)
+{
+ btDbvtNode* node=createnode(pdbvt,parent,data);
+ Merge(volume0,volume1,node->volume);
+ return(node);
+}
+
+//
+static void insertleaf( btDbvt* pdbvt,
+ btDbvtNode* root,
+ btDbvtNode* leaf)
+{
+ if(!pdbvt->m_root)
+ {
+ pdbvt->m_root = leaf;
+ leaf->parent = 0;
+ }
+ else
+ {
+ if(!root->isleaf())
+ {
+ do {
+ root=root->childs[Select( leaf->volume,
+ root->childs[0]->volume,
+ root->childs[1]->volume)];
+ } while(!root->isleaf());
+ }
+ btDbvtNode* prev=root->parent;
+ btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0);
+ if(prev)
+ {
+ prev->childs[indexof(root)] = node;
+ node->childs[0] = root;root->parent=node;
+ node->childs[1] = leaf;leaf->parent=node;
+ do {
+ if(!prev->volume.Contain(node->volume))
+ Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
+ else
+ break;
+ node=prev;
+ } while(0!=(prev=node->parent));
+ }
+ else
+ {
+ node->childs[0] = root;root->parent=node;
+ node->childs[1] = leaf;leaf->parent=node;
+ pdbvt->m_root = node;
+ }
+ }
+}
+
+//
+static btDbvtNode* removeleaf( btDbvt* pdbvt,
+ btDbvtNode* leaf)
+{
+ if(leaf==pdbvt->m_root)
+ {
+ pdbvt->m_root=0;
+ return(0);
+ }
+ else
+ {
+ btDbvtNode* parent=leaf->parent;
+ btDbvtNode* prev=parent->parent;
+ btDbvtNode* sibling=parent->childs[1-indexof(leaf)];
+ if(prev)
+ {
+ prev->childs[indexof(parent)]=sibling;
+ sibling->parent=prev;
+ deletenode(pdbvt,parent);
+ while(prev)
+ {
+ const btDbvtVolume pb=prev->volume;
+ Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
+ if(NotEqual(pb,prev->volume))
+ {
+ prev=prev->parent;
+ } else break;
+ }
+ return(prev?prev:pdbvt->m_root);
+ }
+ else
+ {
+ pdbvt->m_root=sibling;
+ sibling->parent=0;
+ deletenode(pdbvt,parent);
+ return(pdbvt->m_root);
+ }
+ }
+}
+
+//
+static void fetchleaves(btDbvt* pdbvt,
+ btDbvtNode* root,
+ tNodeArray& leaves,
+ int depth=-1)
+{
+ if(root->isinternal()&&depth)
+ {
+ fetchleaves(pdbvt,root->childs[0],leaves,depth-1);
+ fetchleaves(pdbvt,root->childs[1],leaves,depth-1);
+ deletenode(pdbvt,root);
+ }
+ else
+ {
+ leaves.push_back(root);
+ }
+}
+
+//
+static void split( const tNodeArray& leaves,
+ tNodeArray& left,
+ tNodeArray& right,
+ const btVector3& org,
+ const btVector3& axis)
+{
+ left.resize(0);
+ right.resize(0);
+ for(int i=0,ni=leaves.size();i<ni;++i)
+ {
+ if(btDot(axis,leaves[i]->volume.Center()-org)<0)
+ left.push_back(leaves[i]);
+ else
+ right.push_back(leaves[i]);
+ }
+}
+
+//
+static btDbvtVolume bounds( const tNodeArray& leaves)
+{
+#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
+ ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]);
+ btDbvtVolume& volume=*(btDbvtVolume*)locals;
+ volume=leaves[0]->volume;
+#else
+ btDbvtVolume volume=leaves[0]->volume;
+#endif
+ for(int i=1,ni=leaves.size();i<ni;++i)
+ {
+ Merge(volume,leaves[i]->volume,volume);
+ }
+ return(volume);
+}
+
+//
+static void bottomup( btDbvt* pdbvt,
+ tNodeArray& leaves)
+{
+ while(leaves.size()>1)
+ {
+ btScalar minsize=SIMD_INFINITY;
+ int minidx[2]={-1,-1};
+ for(int i=0;i<leaves.size();++i)
+ {
+ for(int j=i+1;j<leaves.size();++j)
+ {
+ const btScalar sz=size(merge(leaves[i]->volume,leaves[j]->volume));
+ if(sz<minsize)
+ {
+ minsize = sz;
+ minidx[0] = i;
+ minidx[1] = j;
+ }
+ }
+ }
+ btDbvtNode* n[] = {leaves[minidx[0]],leaves[minidx[1]]};
+ btDbvtNode* p = createnode(pdbvt,0,n[0]->volume,n[1]->volume,0);
+ p->childs[0] = n[0];
+ p->childs[1] = n[1];
+ n[0]->parent = p;
+ n[1]->parent = p;
+ leaves[minidx[0]] = p;
+ leaves.swap(minidx[1],leaves.size()-1);
+ leaves.pop_back();
+ }
+}
+
+//
+static btDbvtNode* topdown(btDbvt* pdbvt,
+ tNodeArray& leaves,
+ int bu_treshold)
+{
+ static const btVector3 axis[]={btVector3(1,0,0),
+ btVector3(0,1,0),
+ btVector3(0,0,1)};
+ if(leaves.size()>1)
+ {
+ if(leaves.size()>bu_treshold)
+ {
+ const btDbvtVolume vol=bounds(leaves);
+ const btVector3 org=vol.Center();
+ tNodeArray sets[2];
+ int bestaxis=-1;
+ int bestmidp=leaves.size();
+ int splitcount[3][2]={{0,0},{0,0},{0,0}};
+ int i;
+ for( i=0;i<leaves.size();++i)
+ {
+ const btVector3 x=leaves[i]->volume.Center()-org;
+ for(int j=0;j<3;++j)
+ {
+ ++splitcount[j][btDot(x,axis[j])>0?1:0];
+ }
+ }
+ for( i=0;i<3;++i)
+ {
+ if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
+ {
+ const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1]));
+ if(midp<bestmidp)
+ {
+ bestaxis=i;
+ bestmidp=midp;
+ }
+ }
+ }
+ if(bestaxis>=0)
+ {
+ sets[0].reserve(splitcount[bestaxis][0]);
+ sets[1].reserve(splitcount[bestaxis][1]);
+ split(leaves,sets[0],sets[1],org,axis[bestaxis]);
+ }
+ else
+ {
+ sets[0].reserve(leaves.size()/2+1);
+ sets[1].reserve(leaves.size()/2);
+ for(int i=0,ni=leaves.size();i<ni;++i)
+ {
+ sets[i&1].push_back(leaves[i]);
+ }
+ }
+ btDbvtNode* node=createnode(pdbvt,0,vol,0);
+ node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
+ node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
+ node->childs[0]->parent=node;
+ node->childs[1]->parent=node;
+ return(node);
+ }
+ else
+ {
+ bottomup(pdbvt,leaves);
+ return(leaves[0]);
+ }
+ }
+ return(leaves[0]);
+}
+
+//
+static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r)
+{
+ btDbvtNode* p=n->parent;
+ btAssert(n->isinternal());
+ if(p>n)
+ {
+ const int i=indexof(n);
+ const int j=1-i;
+ btDbvtNode* s=p->childs[j];
+ btDbvtNode* q=p->parent;
+ btAssert(n==p->childs[i]);
+ if(q) q->childs[indexof(p)]=n; else r=n;
+ s->parent=n;
+ p->parent=n;
+ n->parent=q;
+ p->childs[0]=n->childs[0];
+ p->childs[1]=n->childs[1];
+ n->childs[0]->parent=p;
+ n->childs[1]->parent=p;
+ n->childs[i]=p;
+ n->childs[j]=s;
+ btSwap(p->volume,n->volume);
+ return(p);
+ }
+ return(n);
+}
+
+#if 0
+static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count)
+{
+ while(n&&(count--)) n=n->parent;
+ return(n);
+}
+#endif
+
+//
+// Api
+//
+
+//
+btDbvt::btDbvt()
+{
+ m_root = 0;
+ m_free = 0;
+ m_lkhd = -1;
+ m_leaves = 0;
+ m_opath = 0;
+}
+
+//
+btDbvt::~btDbvt()
+{
+ clear();
+}
+
+//
+void btDbvt::clear()
+{
+ if(m_root)
+ recursedeletenode(this,m_root);
+ btAlignedFree(m_free);
+ m_free=0;
+ m_lkhd = -1;
+ m_stkStack.clear();
+ m_opath = 0;
+
+}
+
+//
+void btDbvt::optimizeBottomUp()
+{
+ if(m_root)
+ {
+ tNodeArray leaves;
+ leaves.reserve(m_leaves);
+ fetchleaves(this,m_root,leaves);
+ bottomup(this,leaves);
+ m_root=leaves[0];
+ }
+}
+
+//
+void btDbvt::optimizeTopDown(int bu_treshold)
+{
+ if(m_root)
+ {
+ tNodeArray leaves;
+ leaves.reserve(m_leaves);
+ fetchleaves(this,m_root,leaves);
+ m_root=topdown(this,leaves,bu_treshold);
+ }
+}
+
+//
+void btDbvt::optimizeIncremental(int passes)
+{
+ if(passes<0) passes=m_leaves;
+ if(m_root&&(passes>0))
+ {
+ do {
+ btDbvtNode* node=m_root;
+ unsigned bit=0;
+ while(node->isinternal())
+ {
+ node=sort(node,m_root)->childs[(m_opath>>bit)&1];
+ bit=(bit+1)&(sizeof(unsigned)*8-1);
+ }
+ update(node);
+ ++m_opath;
+ } while(--passes);
+ }
+}
+
+//
+btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data)
+{
+ btDbvtNode* leaf=createnode(this,0,volume,data);
+ insertleaf(this,m_root,leaf);
+ ++m_leaves;
+ return(leaf);
+}
+
+//
+void btDbvt::update(btDbvtNode* leaf,int lookahead)
+{
+ btDbvtNode* root=removeleaf(this,leaf);
+ if(root)
+ {
+ if(lookahead>=0)
+ {
+ for(int i=0;(i<lookahead)&&root->parent;++i)
+ {
+ root=root->parent;
+ }
+ } else root=m_root;
+ }
+ insertleaf(this,root,leaf);
+}
+
+//
+void btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume)
+{
+ btDbvtNode* root=removeleaf(this,leaf);
+ if(root)
+ {
+ if(m_lkhd>=0)
+ {
+ for(int i=0;(i<m_lkhd)&&root->parent;++i)
+ {
+ root=root->parent;
+ }
+ } else root=m_root;
+ }
+ leaf->volume=volume;
+ insertleaf(this,root,leaf);
+}
+
+//
+bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin)
+{
+ if(leaf->volume.Contain(volume)) return(false);
+ volume.Expand(btVector3(margin,margin,margin));
+ volume.SignedExpand(velocity);
+ update(leaf,volume);
+ return(true);
+}
+
+//
+bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity)
+{
+ if(leaf->volume.Contain(volume)) return(false);
+ volume.SignedExpand(velocity);
+ update(leaf,volume);
+ return(true);
+}
+
+//
+bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin)
+{
+ if(leaf->volume.Contain(volume)) return(false);
+ volume.Expand(btVector3(margin,margin,margin));
+ update(leaf,volume);
+ return(true);
+}
+
+//
+void btDbvt::remove(btDbvtNode* leaf)
+{
+ removeleaf(this,leaf);
+ deletenode(this,leaf);
+ --m_leaves;
+}
+
+//
+void btDbvt::write(IWriter* iwriter) const
+{
+ btDbvtNodeEnumerator nodes;
+ nodes.nodes.reserve(m_leaves*2);
+ enumNodes(m_root,nodes);
+ iwriter->Prepare(m_root,nodes.nodes.size());
+ for(int i=0;i<nodes.nodes.size();++i)
+ {
+ const btDbvtNode* n=nodes.nodes[i];
+ int p=-1;
+ if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
+ if(n->isinternal())
+ {
+ const int c0=nodes.nodes.findLinearSearch(n->childs[0]);
+ const int c1=nodes.nodes.findLinearSearch(n->childs[1]);
+ iwriter->WriteNode(n,i,p,c0,c1);
+ }
+ else
+ {
+ iwriter->WriteLeaf(n,i,p);
+ }
+ }
+}
+
+//
+void btDbvt::clone(btDbvt& dest,IClone* iclone) const
+{
+ dest.clear();
+ if(m_root!=0)
+ {
+ btAlignedObjectArray<sStkCLN> stack;
+ stack.reserve(m_leaves);
+ stack.push_back(sStkCLN(m_root,0));
+ do {
+ const int i=stack.size()-1;
+ const sStkCLN e=stack[i];
+ btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data);
+ stack.pop_back();
+ if(e.parent!=0)
+ e.parent->childs[i&1]=n;
+ else
+ dest.m_root=n;
+ if(e.node->isinternal())
+ {
+ stack.push_back(sStkCLN(e.node->childs[0],n));
+ stack.push_back(sStkCLN(e.node->childs[1],n));
+ }
+ else
+ {
+ iclone->CloneLeaf(n);
+ }
+ } while(stack.size()>0);
+ }
+}
+
+//
+int btDbvt::maxdepth(const btDbvtNode* node)
+{
+ int depth=0;
+ if(node) getmaxdepth(node,1,depth);
+ return(depth);
+}
+
+//
+int btDbvt::countLeaves(const btDbvtNode* node)
+{
+ if(node->isinternal())
+ return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
+ else
+ return(1);
+}
+
+//
+void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves)
+{
+ if(node->isinternal())
+ {
+ extractLeaves(node->childs[0],leaves);
+ extractLeaves(node->childs[1],leaves);
+ }
+ else
+ {
+ leaves.push_back(node);
+ }
+}
+
+//
+#if DBVT_ENABLE_BENCHMARK
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "LinearMath/btQuickProf.h"
+
+/*
+q6600,2.4ghz
+
+/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32"
+/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch"
+/Fo"..\..\out\release8\build\libbulletcollision\\"
+/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb"
+/W3 /nologo /c /Wp64 /Zi /errorReport:prompt
+
+Benchmarking dbvt...
+World scale: 100.000000
+Extents base: 1.000000
+Extents range: 4.000000
+Leaves: 8192
+sizeof(btDbvtVolume): 32 bytes
+sizeof(btDbvtNode): 44 bytes
+[1] btDbvtVolume intersections: 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::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)
+[11] optimize (incremental): 2514 ms (0%),(1668000 o/s)
+[12] btDbvtVolume notequal: 3659 ms (0%)
+[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s)
+[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s)
+[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s)
+[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s)
+[17] btDbvtVolume select: 3419 ms (0%)
+*/
+
+struct btDbvtBenchmark
+{
+ struct NilPolicy : btDbvt::ICollide
+ {
+ NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {}
+ void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; }
+ void Process(const btDbvtNode*) { ++m_pcount; }
+ void Process(const btDbvtNode*,btScalar depth)
+ {
+ ++m_pcount;
+ if(m_checksort)
+ { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); }
+ }
+ int m_pcount;
+ btScalar m_depth;
+ bool m_checksort;
+ };
+ struct P14 : btDbvt::ICollide
+ {
+ struct Node
+ {
+ const btDbvtNode* leaf;
+ btScalar depth;
+ };
+ void Process(const btDbvtNode* leaf,btScalar depth)
+ {
+ Node n;
+ n.leaf = leaf;
+ n.depth = depth;
+ }
+ static int sortfnc(const Node& a,const Node& b)
+ {
+ if(a.depth<b.depth) return(+1);
+ if(a.depth>b.depth) return(-1);
+ return(0);
+ }
+ btAlignedObjectArray<Node> m_nodes;
+ };
+ struct P15 : btDbvt::ICollide
+ {
+ struct Node
+ {
+ const btDbvtNode* leaf;
+ btScalar depth;
+ };
+ void Process(const btDbvtNode* leaf)
+ {
+ Node n;
+ n.leaf = leaf;
+ n.depth = dot(leaf->volume.Center(),m_axis);
+ }
+ static int sortfnc(const Node& a,const Node& b)
+ {
+ if(a.depth<b.depth) return(+1);
+ if(a.depth>b.depth) return(-1);
+ return(0);
+ }
+ btAlignedObjectArray<Node> m_nodes;
+ btVector3 m_axis;
+ };
+ static btScalar RandUnit()
+ {
+ return(rand()/(btScalar)RAND_MAX);
+ }
+ static btVector3 RandVector3()
+ {
+ return(btVector3(RandUnit(),RandUnit(),RandUnit()));
+ }
+ static btVector3 RandVector3(btScalar cs)
+ {
+ return(RandVector3()*cs-btVector3(cs,cs,cs)/2);
+ }
+ static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es)
+ {
+ return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es));
+ }
+ static btTransform RandTransform(btScalar cs)
+ {
+ btTransform t;
+ t.setOrigin(RandVector3(cs));
+ t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized());
+ return(t);
+ }
+ static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt)
+ {
+ dbvt.clear();
+ for(int i=0;i<leaves;++i)
+ {
+ dbvt.insert(RandVolume(cs,eb,es),0);
+ }
+ }
+};
+
+void btDbvt::benchmark()
+{
+ static const btScalar cfgVolumeCenterScale = 100;
+ static const btScalar cfgVolumeExentsBase = 1;
+ static const btScalar cfgVolumeExentsScale = 4;
+ static const int cfgLeaves = 8192;
+ static const bool cfgEnable = true;
+
+ //[1] btDbvtVolume intersections
+ bool cfgBenchmark1_Enable = cfgEnable;
+ static const int cfgBenchmark1_Iterations = 8;
+ static const int cfgBenchmark1_Reference = 3499;
+ //[2] btDbvtVolume merges
+ bool cfgBenchmark2_Enable = cfgEnable;
+ static const int cfgBenchmark2_Iterations = 4;
+ static const int cfgBenchmark2_Reference = 1945;
+ //[3] btDbvt::collideTT
+ bool cfgBenchmark3_Enable = cfgEnable;
+ static const int cfgBenchmark3_Iterations = 512;
+ static const int cfgBenchmark3_Reference = 5485;
+ //[4] btDbvt::collideTT self
+ bool cfgBenchmark4_Enable = cfgEnable;
+ static const int cfgBenchmark4_Iterations = 512;
+ static const int cfgBenchmark4_Reference = 2814;
+ //[5] btDbvt::collideTT xform
+ bool cfgBenchmark5_Enable = cfgEnable;
+ static const int cfgBenchmark5_Iterations = 512;
+ static const btScalar cfgBenchmark5_OffsetScale = 2;
+ static const int cfgBenchmark5_Reference = 7379;
+ //[6] btDbvt::collideTT xform,self
+ bool cfgBenchmark6_Enable = cfgEnable;
+ static const int cfgBenchmark6_Iterations = 512;
+ static const btScalar cfgBenchmark6_OffsetScale = 2;
+ static const int cfgBenchmark6_Reference = 7270;
+ //[7] btDbvt::rayTest
+ bool cfgBenchmark7_Enable = cfgEnable;
+ static const int cfgBenchmark7_Passes = 32;
+ static const int cfgBenchmark7_Iterations = 65536;
+ static const int cfgBenchmark7_Reference = 6307;
+ //[8] insert/remove
+ bool cfgBenchmark8_Enable = cfgEnable;
+ static const int cfgBenchmark8_Passes = 32;
+ static const int cfgBenchmark8_Iterations = 65536;
+ static const int cfgBenchmark8_Reference = 2105;
+ //[9] updates (teleport)
+ bool cfgBenchmark9_Enable = cfgEnable;
+ static const int cfgBenchmark9_Passes = 32;
+ static const int cfgBenchmark9_Iterations = 65536;
+ static const int cfgBenchmark9_Reference = 1879;
+ //[10] updates (jitter)
+ bool cfgBenchmark10_Enable = cfgEnable;
+ static const btScalar cfgBenchmark10_Scale = cfgVolumeCenterScale/10000;
+ static const int cfgBenchmark10_Passes = 32;
+ static const int cfgBenchmark10_Iterations = 65536;
+ static const int cfgBenchmark10_Reference = 1244;
+ //[11] optimize (incremental)
+ bool cfgBenchmark11_Enable = cfgEnable;
+ static const int cfgBenchmark11_Passes = 64;
+ static const int cfgBenchmark11_Iterations = 65536;
+ static const int cfgBenchmark11_Reference = 2510;
+ //[12] btDbvtVolume notequal
+ bool cfgBenchmark12_Enable = cfgEnable;
+ static const int cfgBenchmark12_Iterations = 32;
+ static const int cfgBenchmark12_Reference = 3677;
+ //[13] culling(OCL+fullsort)
+ bool cfgBenchmark13_Enable = cfgEnable;
+ static const int cfgBenchmark13_Iterations = 1024;
+ static const int cfgBenchmark13_Reference = 2231;
+ //[14] culling(OCL+qsort)
+ bool cfgBenchmark14_Enable = cfgEnable;
+ static const int cfgBenchmark14_Iterations = 8192;
+ static const int cfgBenchmark14_Reference = 3500;
+ //[15] culling(KDOP+qsort)
+ bool cfgBenchmark15_Enable = cfgEnable;
+ static const int cfgBenchmark15_Iterations = 8192;
+ static const int cfgBenchmark15_Reference = 1151;
+ //[16] insert/remove batch
+ bool cfgBenchmark16_Enable = cfgEnable;
+ static const int cfgBenchmark16_BatchCount = 256;
+ static const int cfgBenchmark16_Passes = 16384;
+ static const int cfgBenchmark16_Reference = 5138;
+ //[17] select
+ bool cfgBenchmark17_Enable = cfgEnable;
+ static const int cfgBenchmark17_Iterations = 4;
+ static const int cfgBenchmark17_Reference = 3390;
+
+ btClock wallclock;
+ printf("Benchmarking dbvt...\r\n");
+ printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
+ printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
+ printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
+ printf("\tLeaves: %u\r\n",cfgLeaves);
+ printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume));
+ printf("\tsizeof(btDbvtNode): %u bytes\r\n",sizeof(btDbvtNode));
+ if(cfgBenchmark1_Enable)
+ {// Benchmark 1
+ srand(380843);
+ btAlignedObjectArray<btDbvtVolume> volumes;
+ btAlignedObjectArray<bool> results;
+ volumes.resize(cfgLeaves);
+ results.resize(cfgLeaves);
+ for(int i=0;i<cfgLeaves;++i)
+ {
+ volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+ }
+ printf("[1] btDbvtVolume intersections: ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark1_Iterations;++i)
+ {
+ for(int j=0;j<cfgLeaves;++j)
+ {
+ for(int k=0;k<cfgLeaves;++k)
+ {
+ results[k]=Intersect(volumes[j],volumes[k]);
+ }
+ }
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time);
+ }
+ if(cfgBenchmark2_Enable)
+ {// Benchmark 2
+ srand(380843);
+ btAlignedObjectArray<btDbvtVolume> volumes;
+ btAlignedObjectArray<btDbvtVolume> results;
+ volumes.resize(cfgLeaves);
+ results.resize(cfgLeaves);
+ for(int i=0;i<cfgLeaves;++i)
+ {
+ volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+ }
+ printf("[2] btDbvtVolume merges: ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark2_Iterations;++i)
+ {
+ for(int j=0;j<cfgLeaves;++j)
+ {
+ for(int k=0;k<cfgLeaves;++k)
+ {
+ Merge(volumes[j],volumes[k],results[k]);
+ }
+ }
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time);
+ }
+ if(cfgBenchmark3_Enable)
+ {// Benchmark 3
+ srand(380843);
+ btDbvt dbvt[2];
+ btDbvtBenchmark::NilPolicy policy;
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
+ dbvt[0].optimizeTopDown();
+ dbvt[1].optimizeTopDown();
+ printf("[3] btDbvt::collideTT: ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark3_Iterations;++i)
+ {
+ btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy);
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time);
+ }
+ if(cfgBenchmark4_Enable)
+ {// Benchmark 4
+ srand(380843);
+ btDbvt dbvt;
+ btDbvtBenchmark::NilPolicy policy;
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ printf("[4] btDbvt::collideTT self: ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark4_Iterations;++i)
+ {
+ btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy);
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time);
+ }
+ if(cfgBenchmark5_Enable)
+ {// Benchmark 5
+ srand(380843);
+ btDbvt dbvt[2];
+ btAlignedObjectArray<btTransform> transforms;
+ btDbvtBenchmark::NilPolicy policy;
+ transforms.resize(cfgBenchmark5_Iterations);
+ for(int i=0;i<transforms.size();++i)
+ {
+ transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
+ }
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
+ dbvt[0].optimizeTopDown();
+ dbvt[1].optimizeTopDown();
+ printf("[5] btDbvt::collideTT xform: ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark5_Iterations;++i)
+ {
+ btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy);
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time);
+ }
+ if(cfgBenchmark6_Enable)
+ {// Benchmark 6
+ srand(380843);
+ btDbvt dbvt;
+ btAlignedObjectArray<btTransform> transforms;
+ btDbvtBenchmark::NilPolicy policy;
+ transforms.resize(cfgBenchmark6_Iterations);
+ for(int i=0;i<transforms.size();++i)
+ {
+ transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
+ }
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ printf("[6] btDbvt::collideTT xform,self: ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark6_Iterations;++i)
+ {
+ btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy);
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time);
+ }
+ if(cfgBenchmark7_Enable)
+ {// Benchmark 7
+ srand(380843);
+ btDbvt dbvt;
+ btAlignedObjectArray<btVector3> rayorg;
+ btAlignedObjectArray<btVector3> raydir;
+ btDbvtBenchmark::NilPolicy policy;
+ rayorg.resize(cfgBenchmark7_Iterations);
+ raydir.resize(cfgBenchmark7_Iterations);
+ for(int i=0;i<rayorg.size();++i)
+ {
+ rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
+ raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
+ }
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ printf("[7] btDbvt::rayTest: ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark7_Passes;++i)
+ {
+ for(int j=0;j<cfgBenchmark7_Iterations;++j)
+ {
+ btDbvt::rayTest(dbvt.m_root,rayorg[j],rayorg[j]+raydir[j],policy);
+ }
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ unsigned rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations;
+ printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time);
+ }
+ if(cfgBenchmark8_Enable)
+ {// Benchmark 8
+ srand(380843);
+ btDbvt dbvt;
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ printf("[8] insert/remove: ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark8_Passes;++i)
+ {
+ for(int j=0;j<cfgBenchmark8_Iterations;++j)
+ {
+ dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
+ }
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ const int ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations;
+ printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time);
+ }
+ if(cfgBenchmark9_Enable)
+ {// Benchmark 9
+ srand(380843);
+ btDbvt dbvt;
+ btAlignedObjectArray<const btDbvtNode*> leaves;
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ dbvt.extractLeaves(dbvt.m_root,leaves);
+ printf("[9] updates (teleport): ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark9_Passes;++i)
+ {
+ for(int j=0;j<cfgBenchmark9_Iterations;++j)
+ {
+ dbvt.update(const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]),
+ btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale));
+ }
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations;
+ printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time);
+ }
+ if(cfgBenchmark10_Enable)
+ {// Benchmark 10
+ srand(380843);
+ btDbvt dbvt;
+ btAlignedObjectArray<const btDbvtNode*> leaves;
+ btAlignedObjectArray<btVector3> vectors;
+ vectors.resize(cfgBenchmark10_Iterations);
+ for(int i=0;i<vectors.size();++i)
+ {
+ vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale;
+ }
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ dbvt.extractLeaves(dbvt.m_root,leaves);
+ printf("[10] updates (jitter): ");
+ wallclock.reset();
+
+ for(int i=0;i<cfgBenchmark10_Passes;++i)
+ {
+ for(int j=0;j<cfgBenchmark10_Iterations;++j)
+ {
+ const btVector3& d=vectors[j];
+ btDbvtNode* l=const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]);
+ btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d);
+ dbvt.update(l,v);
+ }
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations;
+ printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time);
+ }
+ if(cfgBenchmark11_Enable)
+ {// Benchmark 11
+ srand(380843);
+ btDbvt dbvt;
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ printf("[11] optimize (incremental): ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark11_Passes;++i)
+ {
+ dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ const int op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations;
+ printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000);
+ }
+ if(cfgBenchmark12_Enable)
+ {// Benchmark 12
+ srand(380843);
+ btAlignedObjectArray<btDbvtVolume> volumes;
+ btAlignedObjectArray<bool> results;
+ volumes.resize(cfgLeaves);
+ results.resize(cfgLeaves);
+ for(int i=0;i<cfgLeaves;++i)
+ {
+ volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+ }
+ printf("[12] btDbvtVolume notequal: ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark12_Iterations;++i)
+ {
+ for(int j=0;j<cfgLeaves;++j)
+ {
+ for(int k=0;k<cfgLeaves;++k)
+ {
+ results[k]=NotEqual(volumes[j],volumes[k]);
+ }
+ }
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time);
+ }
+ if(cfgBenchmark13_Enable)
+ {// Benchmark 13
+ srand(380843);
+ btDbvt dbvt;
+ btAlignedObjectArray<btVector3> vectors;
+ btDbvtBenchmark::NilPolicy policy;
+ vectors.resize(cfgBenchmark13_Iterations);
+ for(int i=0;i<vectors.size();++i)
+ {
+ vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+ }
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ printf("[13] culling(OCL+fullsort): ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark13_Iterations;++i)
+ {
+ static const btScalar offset=0;
+ policy.m_depth=-SIMD_INFINITY;
+ dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy);
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ const int t=cfgBenchmark13_Iterations;
+ printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time);
+ }
+ if(cfgBenchmark14_Enable)
+ {// Benchmark 14
+ srand(380843);
+ btDbvt dbvt;
+ btAlignedObjectArray<btVector3> vectors;
+ btDbvtBenchmark::P14 policy;
+ vectors.resize(cfgBenchmark14_Iterations);
+ for(int i=0;i<vectors.size();++i)
+ {
+ vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+ }
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ policy.m_nodes.reserve(cfgLeaves);
+ printf("[14] culling(OCL+qsort): ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark14_Iterations;++i)
+ {
+ static const btScalar offset=0;
+ policy.m_nodes.resize(0);
+ dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false);
+ policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc);
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ const int t=cfgBenchmark14_Iterations;
+ printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time);
+ }
+ if(cfgBenchmark15_Enable)
+ {// Benchmark 15
+ srand(380843);
+ btDbvt dbvt;
+ btAlignedObjectArray<btVector3> vectors;
+ btDbvtBenchmark::P15 policy;
+ vectors.resize(cfgBenchmark15_Iterations);
+ for(int i=0;i<vectors.size();++i)
+ {
+ vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+ }
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ policy.m_nodes.reserve(cfgLeaves);
+ printf("[15] culling(KDOP+qsort): ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark15_Iterations;++i)
+ {
+ static const btScalar offset=0;
+ policy.m_nodes.resize(0);
+ policy.m_axis=vectors[i];
+ dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy);
+ policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc);
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ const int t=cfgBenchmark15_Iterations;
+ printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time);
+ }
+ if(cfgBenchmark16_Enable)
+ {// Benchmark 16
+ srand(380843);
+ btDbvt dbvt;
+ btAlignedObjectArray<btDbvtNode*> batch;
+ btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+ dbvt.optimizeTopDown();
+ batch.reserve(cfgBenchmark16_BatchCount);
+ printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount);
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark16_Passes;++i)
+ {
+ for(int j=0;j<cfgBenchmark16_BatchCount;++j)
+ {
+ batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
+ }
+ for(int j=0;j<cfgBenchmark16_BatchCount;++j)
+ {
+ dbvt.remove(batch[j]);
+ }
+ batch.resize(0);
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ const int ir=cfgBenchmark16_Passes*cfgBenchmark16_BatchCount;
+ printf("%u ms (%i%%),(%u bir/s)\r\n",time,(time-cfgBenchmark16_Reference)*100/time,int(ir*1000.0/time));
+ }
+ if(cfgBenchmark17_Enable)
+ {// Benchmark 17
+ srand(380843);
+ btAlignedObjectArray<btDbvtVolume> volumes;
+ btAlignedObjectArray<int> results;
+ btAlignedObjectArray<int> indices;
+ volumes.resize(cfgLeaves);
+ results.resize(cfgLeaves);
+ indices.resize(cfgLeaves);
+ for(int i=0;i<cfgLeaves;++i)
+ {
+ indices[i]=i;
+ volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+ }
+ for(int i=0;i<cfgLeaves;++i)
+ {
+ btSwap(indices[i],indices[rand()%cfgLeaves]);
+ }
+ printf("[17] btDbvtVolume select: ");
+ wallclock.reset();
+ for(int i=0;i<cfgBenchmark17_Iterations;++i)
+ {
+ for(int j=0;j<cfgLeaves;++j)
+ {
+ for(int k=0;k<cfgLeaves;++k)
+ {
+ const int idx=indices[k];
+ results[idx]=Select(volumes[idx],volumes[j],volumes[k]);
+ }
+ }
+ }
+ const int time=(int)wallclock.getTimeMilliseconds();
+ printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time);
+ }
+ printf("\r\n\r\n");
+}
+#endif
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btDbvtBroadphase.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btDbvtBroadphase.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btDbvtBroadphase.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btDbvtBroadphase.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,728 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///btDbvtBroadphase implementation by Nathanael Presson
+
+#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
+
+//
+// Profiling
+//
+
+#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK
+#include <stdio.h>
+#endif
+
+#if DBVT_BP_PROFILE
+struct ProfileScope
+{
+ __forceinline ProfileScope(btClock& clock,unsigned long& value) :
+ 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_)
+#endif
+
+//
+// Helpers
+//
+
+//
+template <typename T>
+static inline void listappend(T* item,T*& list)
+{
+ item->links[0]=0;
+ item->links[1]=list;
+ if(list) list->links[0]=item;
+ list=item;
+}
+
+//
+template <typename T>
+static inline void listremove(T* item,T*& list)
+{
+ if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
+ if(item->links[1]) item->links[1]->links[0]=item->links[0];
+}
+
+//
+template <typename T>
+static inline int listcount(T* root)
+{
+ int n=0;
+ while(root) { ++n;root=root->links[1]; }
+ return(n);
+}
+
+//
+template <typename T>
+static inline void clear(T& value)
+{
+ static const struct ZeroDummy : T {} zerodummy;
+ value=zerodummy;
+}
+
+//
+// Colliders
+//
+
+/* Tree collider */
+struct btDbvtTreeCollider : btDbvt::ICollide
+{
+ btDbvtBroadphase* pbp;
+ btDbvtProxy* proxy;
+ btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
+ void Process(const btDbvtNode* na,const btDbvtNode* nb)
+ {
+ if(na!=nb)
+ {
+ 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)
+ {
+ Process(n,proxy->leaf);
+ }
+};
+
+//
+// btDbvtBroadphase
+//
+
+//
+btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
+{
+ m_deferedcollide = false;
+ m_needcleanup = true;
+ m_releasepaircache = (paircache!=0)?false:true;
+ m_prediction = 0;
+ 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;
+ }
+#if DBVT_BP_PROFILE
+ clear(m_profiling);
+#endif
+}
+
+//
+btDbvtBroadphase::~btDbvtBroadphase()
+{
+ if(m_releasepaircache)
+ {
+ m_paircache->~btOverlappingPairCache();
+ btAlignedFree(m_paircache);
+ }
+}
+
+//
+btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin,
+ const btVector3& aabbMax,
+ int /*shapeType*/,
+ void* userPtr,
+ short int collisionFilterGroup,
+ short int collisionFilterMask,
+ btDispatcher* /*dispatcher*/,
+ void* /*multiSapProxy*/)
+{
+ btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( 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;
+ m_sets[0].collideTV(m_sets[0].m_root,aabb,collider);
+ m_sets[1].collideTV(m_sets[1].m_root,aabb,collider);
+ }
+ return(proxy);
+}
+
+//
+void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
+ btDispatcher* dispatcher)
+{
+ btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
+ if(proxy->stage==STAGECOUNT)
+ m_sets[1].remove(proxy->leaf);
+ else
+ m_sets[0].remove(proxy->leaf);
+ listremove(proxy,m_stageRoots[proxy->stage]);
+ m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
+ btAlignedFree(proxy);
+ 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*/)
+{
+ btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
+#if DBVT_BP_PREVENTFALSEUPDATE
+ if(NotEqual(aabb,proxy->leaf->volume))
+#endif
+ {
+ 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;
+ }
+ else
+ {/* dynamic set */
+ ++m_updates_call;
+ if(Intersect(proxy->leaf->volume,aabb))
+ {/* Moving */
+
+ 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;
+ }
+ }
+ else
+ {/* Teleporting */
+ m_sets[0].update(proxy->leaf,aabb);
+ ++m_updates_done;
+ docollide=true;
+ }
+ }
+ 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)
+ {
+ 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);
+ }
+ }
+ }
+}
+
+//
+void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+ collide(dispatcher);
+#if DBVT_BP_PROFILE
+ if(0==(m_pid%DBVT_BP_PROFILING_RATE))
+ {
+ printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
+ unsigned int total=m_profiling.m_total;
+ if(total<=0) total=1;
+ printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
+ printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
+ printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
+ printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE);
+ const unsigned long sum=m_profiling.m_ddcollide+
+ m_profiling.m_fdcollide+
+ m_profiling.m_cleanup;
+ printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
+ printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE));
+ clear(m_profiling);
+ m_clock.reset();
+ }
+#endif
+
+ 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;i<overlappingPairArray.size();i++)
+ {
+
+ btBroadphasePair& pair = overlappingPairArray[i];
+
+ bool isDuplicate = (pair == previousPair);
+
+ previousPair = pair;
+
+ bool needsRemoval = false;
+
+ if (!isDuplicate)
+ {
+ //important to perform AABB check that is consistent with the broadphase
+ btDbvtProxy* pa=(btDbvtProxy*)pair.m_pProxy0;
+ btDbvtProxy* pb=(btDbvtProxy*)pair.m_pProxy1;
+ bool hasOverlap = Intersect(pa->leaf->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)
+{
+ /*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());
+ {
+ int i;
+ for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
+ {
+ printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
+ getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
+ }
+ printf("\n");
+ }
+*/
+
+
+
+ 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<int>(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);
+ 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;
+ }
+ /* collide dynamics */
+ {
+ btDbvtTreeCollider collider(this);
+ if(m_deferedcollide)
+ {
+ SPC(m_profiling.m_fdcollide);
+ m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider);
+ }
+ if(m_deferedcollide)
+ {
+ 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)
+ {
+ SPC(m_profiling.m_cleanup);
+ btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
+ if(pairs.size()>0)
+ {
+
+ int ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
+ for(int i=0;i<ni;++i)
+ {
+ 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->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;
+ }
+ }
+ ++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;
+}
+
+//
+void btDbvtBroadphase::optimize()
+{
+ m_sets[0].optimizeTopDown();
+ m_sets[1].optimizeTopDown();
+}
+
+//
+btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache()
+{
+ return(m_paircache);
+}
+
+//
+const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const
+{
+ return(m_paircache);
+}
+
+//
+void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+{
+
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds;
+
+ if(!m_sets[0].empty())
+ if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume,
+ m_sets[1].m_root->volume,bounds);
+ else
+ bounds=m_sets[0].m_root->volume;
+ else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume;
+ else
+ bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
+ aabbMin=bounds.Mins();
+ aabbMax=bounds.Maxs();
+}
+
+void btDbvtBroadphase::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_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;
+ }
+ }
+}
+
+//
+void btDbvtBroadphase::printStats()
+{}
+
+//
+#if DBVT_BP_ENABLE_BENCHMARK
+
+struct btBroadphaseBenchmark
+{
+ struct Experiment
+ {
+ const char* name;
+ int object_count;
+ int update_count;
+ int spawn_count;
+ 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;
+ center[1] = btCos(time*(btScalar)1.38)*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
+ printf("%s : %u us (%u ms)\r\n",name,us,ms);
+ }
+};
+
+void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
+{
+ 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},*/
+ };
+ static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]);
+ btAlignedObjectArray<btBroadphaseBenchmark::Object*> objects;
+ btClock wallclock;
+ /* Begin */
+ for(int iexp=0;iexp<nexperiments;++iexp)
+ {
+ const btBroadphaseBenchmark::Experiment& experiment=experiments[iexp];
+ const int object_count=experiment.object_count;
+ const int update_count=(object_count*experiment.update_count)/100;
+ const int spawn_count=(object_count*experiment.spawn_count)/100;
+ const btScalar speed=experiment.speed;
+ const btScalar amplitude=experiment.amplitude;
+ printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
+ printf("\tObjects: %u\r\n",object_count);
+ printf("\tUpdate: %u\r\n",update_count);
+ printf("\tSpawn: %u\r\n",spawn_count);
+ printf("\tSpeed: %f\r\n",speed);
+ printf("\tAmplitude: %f\r\n",amplitude);
+ srand(180673);
+ /* Create objects */
+ wallclock.reset();
+ objects.reserve(object_count);
+ for(int i=0;i<object_count;++i)
+ {
+ 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;i<objects.size();++i)
+ {
+ objects[i]->update(speed,amplitude,pbi);
+ }
+ btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
+ /* Updates */
+ wallclock.reset();
+ for(int i=0;i<experiment.iterations;++i)
+ {
+ for(int j=0;j<update_count;++j)
+ {
+ objects[j]->update(speed,amplitude,pbi);
+ }
+ pbi->calculateOverlappingPairs(0);
+ }
+ btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
+ /* Clean up */
+ wallclock.reset();
+ for(int i=0;i<objects.size();++i)
+ {
+ pbi->destroyProxy(objects[i]->proxy,0);
+ delete objects[i];
+ }
+ objects.resize(0);
+ btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
+ }
+
+}
+#else
+void btDbvtBroadphase::benchmark(btBroadphaseInterface*)
+{}
+#endif
+
+#if DBVT_BP_PROFILE
+#undef SPC
+#endif
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btDefaultCollisionConfiguration.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btDefaultCollisionConfiguration.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btDefaultCollisionConfiguration.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btDefaultCollisionConfiguration.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,298 @@
+/*
+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 "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
+
+#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+
+
+
+#include "LinearMath/btStackAlloc.h"
+#include "LinearMath/btPoolAllocator.h"
+
+
+
+
+
+btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
+//btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool)
+{
+
+ void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16);
+ m_simplexSolver = new (mem)btVoronoiSimplexSolver();
+
+ if (constructionInfo.m_useEpaPenetrationAlgorithm)
+ {
+ mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
+ m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
+ }else
+ {
+ mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
+ m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
+ }
+
+ //default CreationFunctions, filling the m_doubleDispatch table
+ mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16);
+ m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver);
+ mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
+ m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc;
+ mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
+ m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
+ mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16);
+ m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc;
+ mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16);
+ m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc;
+ mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16);
+ m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc;
+
+ mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16);
+ m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc;
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+ mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
+ m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc;
+ mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
+ m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc;
+ m_boxSphereCF->m_swapped = true;
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+ mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
+ m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
+ mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
+ m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
+ m_triangleSphereCF->m_swapped = true;
+
+ mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16);
+ m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc;
+
+ //convex versus plane
+ mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
+ m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
+ mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
+ m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
+ m_planeConvexCF->m_swapped = true;
+
+ ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
+ int maxSize = sizeof(btConvexConvexAlgorithm);
+ int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
+ int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
+ int sl = sizeof(btConvexSeparatingDistanceUtil);
+ sl = sizeof(btGjkPairDetector);
+ int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
+ collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
+ collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
+
+ if (constructionInfo.m_stackAlloc)
+ {
+ m_ownsStackAllocator = false;
+ this->m_stackAlloc = constructionInfo.m_stackAlloc;
+ } else
+ {
+ m_ownsStackAllocator = true;
+ void* mem = btAlignedAlloc(sizeof(btStackAlloc),16);
+ m_stackAlloc = new(mem)btStackAlloc(constructionInfo.m_defaultStackAllocatorSize);
+ }
+
+ if (constructionInfo.m_persistentManifoldPool)
+ {
+ m_ownsPersistentManifoldPool = false;
+ m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool;
+ } else
+ {
+ m_ownsPersistentManifoldPool = true;
+ void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
+ m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize);
+ }
+
+ if (constructionInfo.m_collisionAlgorithmPool)
+ {
+ m_ownsCollisionAlgorithmPool = false;
+ m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool;
+ } else
+ {
+ m_ownsCollisionAlgorithmPool = true;
+ void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
+ m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
+ }
+
+
+}
+
+btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
+{
+ if (m_ownsStackAllocator)
+ {
+ m_stackAlloc->destroy();
+ m_stackAlloc->~btStackAlloc();
+ btAlignedFree(m_stackAlloc);
+ }
+ if (m_ownsCollisionAlgorithmPool)
+ {
+ m_collisionAlgorithmPool->~btPoolAllocator();
+ btAlignedFree(m_collisionAlgorithmPool);
+ }
+ if (m_ownsPersistentManifoldPool)
+ {
+ m_persistentManifoldPool->~btPoolAllocator();
+ btAlignedFree(m_persistentManifoldPool);
+ }
+
+ m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_convexConvexCreateFunc);
+
+ m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_convexConcaveCreateFunc);
+ m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_swappedConvexConcaveCreateFunc);
+
+ m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_compoundCreateFunc);
+
+ m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_swappedCompoundCreateFunc);
+
+ m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_emptyCreateFunc);
+
+ m_sphereSphereCF->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_sphereSphereCF);
+
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+ m_sphereBoxCF->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_sphereBoxCF);
+ m_boxSphereCF->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_boxSphereCF);
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+ m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_sphereTriangleCF);
+ m_triangleSphereCF->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_triangleSphereCF);
+ m_boxBoxCF->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_boxBoxCF);
+
+ m_convexPlaneCF->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_convexPlaneCF);
+ m_planeConvexCF->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_planeConvexCF);
+
+ m_simplexSolver->~btVoronoiSimplexSolver();
+ btAlignedFree(m_simplexSolver);
+
+ m_pdSolver->~btConvexPenetrationDepthSolver();
+
+ btAlignedFree(m_pdSolver);
+
+
+}
+
+
+btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
+{
+
+
+
+ if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+ {
+ return m_sphereSphereCF;
+ }
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+ if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE))
+ {
+ return m_sphereBoxCF;
+ }
+
+ if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+ {
+ return m_boxSphereCF;
+ }
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+
+ if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE))
+ {
+ return m_sphereTriangleCF;
+ }
+
+ if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+ {
+ return m_triangleSphereCF;
+ }
+
+ if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
+ {
+ return m_boxBoxCF;
+ }
+
+ if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
+ {
+ return m_convexPlaneCF;
+ }
+
+ if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE))
+ {
+ return m_planeConvexCF;
+ }
+
+
+
+ if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
+ {
+ return m_convexConvexCreateFunc;
+ }
+
+ if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
+ {
+ return m_convexConcaveCreateFunc;
+ }
+
+ if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
+ {
+ return m_swappedConvexConcaveCreateFunc;
+ }
+
+ if (btBroadphaseProxy::isCompound(proxyType0))
+ {
+ return m_compoundCreateFunc;
+ } else
+ {
+ if (btBroadphaseProxy::isCompound(proxyType1))
+ {
+ return m_swappedCompoundCreateFunc;
+ }
+ }
+
+ //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;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btDiscreteDynamicsWorld.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btDiscreteDynamicsWorld.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btDiscreteDynamicsWorld.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btDiscreteDynamicsWorld.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1421 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+
+//collision detection
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btQuickprof.h"
+
+//rigidbody & constraints
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#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"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+#include "LinearMath/btIDebugDraw.h"
+
+
+#include "BulletDynamics/Dynamics/btActionInterface.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btMotionState.h"
+
+
+
+
+
+btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
+m_constraintSolver(constraintSolver),
+m_gravity(0,-10,0),
+m_localTime(btScalar(1.)/btScalar(60.)),
+m_synchronizeAllMotionStates(false),
+m_profileTimings(0)
+{
+ if (!m_constraintSolver)
+ {
+ void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
+ m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
+ m_ownsConstraintSolver = true;
+ } else
+ {
+ m_ownsConstraintSolver = false;
+ }
+
+ {
+ void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
+ m_islandManager = new (mem) btSimulationIslandManager();
+ }
+
+ m_ownsIslandManager = true;
+}
+
+
+btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
+{
+ //only delete it when we created it
+ if (m_ownsIslandManager)
+ {
+ m_islandManager->~btSimulationIslandManager();
+ btAlignedFree( m_islandManager);
+ }
+ if (m_ownsConstraintSolver)
+ {
+
+ m_constraintSolver->~btConstraintSolver();
+ btAlignedFree(m_constraintSolver);
+ }
+}
+
+void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
+{
+///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
+///to switch status _after_ adding kinematic objects to the world
+///fix it for Bullet 3.x release
+ for (int i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+ btRigidBody* body = btRigidBody::upcast(colObj);
+ if (body && body->getActivationState() != ISLAND_SLEEPING)
+ {
+ if (body->isKinematicObject())
+ {
+ //to calculate velocities next frame
+ body->saveKinematicState(timeStep);
+ }
+ }
+ }
+
+}
+
+void btDiscreteDynamicsWorld::debugDrawWorld()
+{
+ BT_PROFILE("debugDrawWorld");
+
+ if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+ {
+ int numManifolds = getDispatcher()->getNumManifolds();
+ btVector3 color(0,0,0);
+ for (int i=0;i<numManifolds;i++)
+ {
+ btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+ //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+ //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+
+ int numContacts = contactManifold->getNumContacts();
+ for (int j=0;j<numContacts;j++)
+ {
+ btManifoldPoint& cp = contactManifold->getContactPoint(j);
+ getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+ }
+ }
+ }
+ 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;
+
+ for ( i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+ if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+ {
+ btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
+ switch(colObj->getActivationState())
+ {
+ case ACTIVE_TAG:
+ color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
+ case ISLAND_SLEEPING:
+ color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
+ case WANTS_DEACTIVATION:
+ color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
+ case DISABLE_DEACTIVATION:
+ color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
+ case DISABLE_SIMULATION:
+ color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
+ default:
+ {
+ color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
+ }
+ };
+
+ debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+ }
+ if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+ {
+ btVector3 minAabb,maxAabb;
+ btVector3 colorvec(1,0,0);
+ colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+ m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+ }
+
+ }
+
+ if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
+ {
+ for (i=0;i<m_actions.size();i++)
+ {
+ m_actions[i]->debugDraw(m_debugDrawer);
+ }
+ }
+ }
+}
+
+void btDiscreteDynamicsWorld::clearForces()
+{
+ ///@todo: iterate over awake simulation islands!
+ for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+ {
+ btRigidBody* body = m_nonStaticRigidBodies[i];
+ //need to check if next line is ok
+ //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
+ body->clearForces();
+ }
+}
+
+///apply gravity, call this once per timestep
+void btDiscreteDynamicsWorld::applyGravity()
+{
+ ///@todo: iterate over awake simulation islands!
+ for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+ {
+ btRigidBody* body = m_nonStaticRigidBodies[i];
+ if (body->isActive())
+ {
+ body->applyGravity();
+ }
+ }
+}
+
+
+void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
+{
+ btAssert(body);
+
+ if (body->getMotionState() && !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");
+ if (m_synchronizeAllMotionStates)
+ {
+ //iterate over all collision objects
+ for ( int i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+ btRigidBody* body = btRigidBody::upcast(colObj);
+ if (body)
+ synchronizeSingleMotionState(body);
+ }
+ } else
+ {
+ //iterate over all active rigid bodies
+ for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+ {
+ btRigidBody* body = m_nonStaticRigidBodies[i];
+ if (body->isActive())
+ synchronizeSingleMotionState(body);
+ }
+ }
+}
+
+
+int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
+{
+ startProfiling(timeStep);
+
+ BT_PROFILE("stepSimulation");
+
+ int numSimulationSubSteps = 0;
+
+ if (maxSubSteps)
+ {
+ //fixed timestep with interpolation
+ m_localTime += timeStep;
+ if (m_localTime >= fixedTimeStep)
+ {
+ numSimulationSubSteps = int( m_localTime / fixedTimeStep);
+ m_localTime -= numSimulationSubSteps * fixedTimeStep;
+ }
+ } else
+ {
+ //variable timestep
+ fixedTimeStep = timeStep;
+ m_localTime = timeStep;
+ if (btFuzzyZero(timeStep))
+ {
+ numSimulationSubSteps = 0;
+ maxSubSteps = 0;
+ } else
+ {
+ numSimulationSubSteps = 1;
+ maxSubSteps = 1;
+ }
+ }
+
+ //process some debugging flags
+ if (getDebugDrawer())
+ {
+ btIDebugDraw* debugDrawer = getDebugDrawer ();
+ gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
+ }
+ if (numSimulationSubSteps)
+ {
+
+ saveKinematicState(fixedTimeStep);
+
+ applyGravity();
+
+ //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
+ int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
+
+ for (int i=0;i<clampedSimulationSteps;i++)
+ {
+ internalSingleStepSimulation(fixedTimeStep);
+ synchronizeMotionStates();
+ }
+
+ } else
+ {
+ synchronizeMotionStates();
+ }
+
+ clearForces();
+
+#ifndef BT_NO_PROFILE
+ CProfileManager::Increment_Frame_Counter();
+#endif //BT_NO_PROFILE
+
+ return numSimulationSubSteps;
+}
+
+void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
+{
+
+ BT_PROFILE("internalSingleStepSimulation");
+
+ if(0 != m_internalPreTickCallback) {
+ (*m_internalPreTickCallback)(this, timeStep);
+ }
+
+ ///apply gravity, predict motion
+ predictUnconstraintMotion(timeStep);
+
+ btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
+ dispatchInfo.m_timeStep = timeStep;
+ dispatchInfo.m_stepCount = 0;
+ dispatchInfo.m_debugDraw = getDebugDrawer();
+
+ ///perform collision detection
+ performDiscreteCollisionDetection();
+
+ calculateSimulationIslands();
+
+
+ getSolverInfo().m_timeStep = timeStep;
+
+
+
+ ///solve contact and other joint constraints
+ solveConstraints(getSolverInfo());
+
+ ///CallbackTriggers();
+
+ ///integrate transforms
+ integrateTransforms(timeStep);
+
+ ///update vehicle simulation
+ updateActions(timeStep);
+
+ updateActivationState( timeStep );
+
+ if(0 != m_internalTickCallback) {
+ (*m_internalTickCallback)(this, timeStep);
+ }
+}
+
+void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
+{
+ m_gravity = gravity;
+ for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+ {
+ btRigidBody* body = m_nonStaticRigidBodies[i];
+ if (body->isActive())
+ {
+ body->setGravity(gravity);
+ }
+ }
+}
+
+btVector3 btDiscreteDynamicsWorld::getGravity () const
+{
+ return m_gravity;
+}
+
+void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+{
+ btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);
+}
+
+void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+ btRigidBody* body = btRigidBody::upcast(collisionObject);
+ if (body)
+ removeRigidBody(body);
+ else
+ btCollisionWorld::removeCollisionObject(collisionObject);
+}
+
+void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
+{
+ m_nonStaticRigidBodies.remove(body);
+ btCollisionWorld::removeCollisionObject(body);
+}
+
+
+void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
+{
+ if (!body->isStaticOrKinematicObject())
+ {
+ body->setGravity(m_gravity);
+ }
+
+ if (body->getCollisionShape())
+ {
+ if (!body->isStaticObject())
+ {
+ m_nonStaticRigidBodies.push_back(body);
+ } else
+ {
+ body->setActivationState(ISLAND_SLEEPING);
+ }
+
+ bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
+ short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
+ short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+ addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
+ }
+}
+
+void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+{
+ if (!body->isStaticOrKinematicObject())
+ {
+ body->setGravity(m_gravity);
+ }
+
+ if (body->getCollisionShape())
+ {
+ if (!body->isStaticObject())
+ {
+ m_nonStaticRigidBodies.push_back(body);
+ }
+ else
+ {
+ body->setActivationState(ISLAND_SLEEPING);
+ }
+ addCollisionObject(body,group,mask);
+ }
+}
+
+
+void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
+{
+ BT_PROFILE("updateActions");
+
+ for ( int i=0;i<m_actions.size();i++)
+ {
+ m_actions[i]->updateAction( this, timeStep);
+ }
+}
+
+
+void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
+{
+ BT_PROFILE("updateActivationState");
+
+ for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+ {
+ btRigidBody* body = m_nonStaticRigidBodies[i];
+ if (body)
+ {
+ body->updateDeactivation(timeStep);
+
+ if (body->wantsSleeping())
+ {
+ if (body->isStaticOrKinematicObject())
+ {
+ body->setActivationState(ISLAND_SLEEPING);
+ } else
+ {
+ if (body->getActivationState() == ACTIVE_TAG)
+ body->setActivationState( WANTS_DEACTIVATION );
+ if (body->getActivationState() == ISLAND_SLEEPING)
+ {
+ body->setAngularVelocity(btVector3(0,0,0));
+ body->setLinearVelocity(btVector3(0,0,0));
+ }
+
+ }
+ } else
+ {
+ if (body->getActivationState() != DISABLE_DEACTIVATION)
+ body->setActivationState( ACTIVE_TAG );
+ }
+ }
+ }
+}
+
+void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
+{
+ m_constraints.push_back(constraint);
+ if (disableCollisionsBetweenLinkedBodies)
+ {
+ constraint->getRigidBodyA().addConstraintRef(constraint);
+ constraint->getRigidBodyB().addConstraintRef(constraint);
+ }
+}
+
+void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
+{
+ m_constraints.remove(constraint);
+ constraint->getRigidBodyA().removeConstraintRef(constraint);
+ constraint->getRigidBodyB().removeConstraintRef(constraint);
+}
+
+void btDiscreteDynamicsWorld::addAction(btActionInterface* action)
+{
+ m_actions.push_back(action);
+}
+
+void btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
+{
+ m_actions.remove(action);
+}
+
+
+void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
+{
+ addAction(vehicle);
+}
+
+void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
+{
+ removeAction(vehicle);
+}
+
+void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
+{
+ addAction(character);
+}
+
+void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
+{
+ removeAction(character);
+}
+
+
+SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
+{
+ int islandId;
+
+ const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+ const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+ islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
+ return islandId;
+
+}
+
+
+class btSortConstraintOnIslandPredicate
+{
+ public:
+
+ bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
+ {
+ int rIslandId0,lIslandId0;
+ rIslandId0 = btGetConstraintIslandId(rhs);
+ lIslandId0 = btGetConstraintIslandId(lhs);
+ return lIslandId0 < rIslandId0;
+ }
+};
+
+
+
+
+void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
+{
+ BT_PROFILE("solveConstraints");
+
+ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
+ {
+
+ btContactSolverInfo& m_solverInfo;
+ btConstraintSolver* m_solver;
+ btTypedConstraint** m_sortedConstraints;
+ int m_numConstraints;
+ btIDebugDraw* m_debugDrawer;
+ btStackAlloc* m_stackAlloc;
+ btDispatcher* m_dispatcher;
+
+ InplaceSolverIslandCallback(
+ btContactSolverInfo& solverInfo,
+ btConstraintSolver* solver,
+ btTypedConstraint** sortedConstraints,
+ int numConstraints,
+ btIDebugDraw* debugDrawer,
+ btStackAlloc* stackAlloc,
+ btDispatcher* dispatcher)
+ :m_solverInfo(solverInfo),
+ m_solver(solver),
+ m_sortedConstraints(sortedConstraints),
+ m_numConstraints(numConstraints),
+ m_debugDrawer(debugDrawer),
+ m_stackAlloc(stackAlloc),
+ m_dispatcher(dispatcher)
+ {
+
+ }
+
+ InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
+ {
+ btAssert(0);
+ (void)other;
+ return *this;
+ }
+ virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
+ {
+ if (islandId<0)
+ {
+ 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
+ btTypedConstraint** startConstraint = 0;
+ int numCurConstraints = 0;
+ int i;
+
+ //find the first constraint for this island
+ for (i=0;i<m_numConstraints;i++)
+ {
+ if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+ {
+ startConstraint = &m_sortedConstraints[i];
+ break;
+ }
+ }
+ //count the number of constraints in this island
+ for (;i<m_numConstraints;i++)
+ {
+ if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+ {
+ numCurConstraints++;
+ }
+ }
+
+ ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
+ if (numManifolds + numCurConstraints)
+ {
+ m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+ }
+
+ }
+ }
+
+ };
+
+ //sorted version of all btTypedConstraint, based on islandId
+ btAlignedObjectArray<btTypedConstraint*> sortedConstraints;
+ sortedConstraints.resize( m_constraints.size());
+ int i;
+ for (i=0;i<getNumConstraints();i++)
+ {
+ sortedConstraints[i] = m_constraints[i];
+ }
+
+// btAssert(0);
+
+
+
+ sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
+
+ btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
+
+ InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc,m_dispatcher1);
+
+ m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+
+ /// solve all the constraints for this island
+ m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
+
+ m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
+}
+
+
+
+
+void btDiscreteDynamicsWorld::calculateSimulationIslands()
+{
+ BT_PROFILE("calculateSimulationIslands");
+
+ getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
+
+ {
+ int i;
+ int numConstraints = int(m_constraints.size());
+ for (i=0;i< numConstraints ; i++ )
+ {
+ btTypedConstraint* constraint = m_constraints[i];
+
+ const btRigidBody* colObj0 = &constraint->getRigidBodyA();
+ const btRigidBody* colObj1 = &constraint->getRigidBodyB();
+
+ if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
+ ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
+ {
+ if (colObj0->isActive() || colObj1->isActive())
+ {
+
+ getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
+ (colObj1)->getIslandTag());
+ }
+ }
+ }
+ }
+
+ //Store the island id in each body
+ getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
+
+
+}
+
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
+{
+ btCollisionObject* m_me;
+ btScalar m_allowedPenetration;
+ btOverlappingPairCache* m_pairCache;
+ btDispatcher* m_dispatcher;
+
+
+public:
+ 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_dispatcher(dispatcher)
+ {
+ }
+
+ virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
+ {
+ if (convexResult.m_hitCollisionObject == m_me)
+ 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;
+ linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
+
+ btVector3 relativeVelocity = (linVelA-linVelB);
+ //don't report time of impact for motion away from the contact normal (or causes minor penetration)
+ if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
+ return 1.f;
+
+ return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
+ }
+
+ virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+ {
+ //don't collide with itself
+ if (proxy0->m_clientObject == m_me)
+ return false;
+
+ ///don't do CCD when the collision filters are not matching
+ if (!ClosestConvexResultCallback::needsCollision(proxy0))
+ return false;
+
+ 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))
+ {
+ ///don't do CCD when there are already contact points (touching contact/penetration)
+ btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+ btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
+ if (collisionPair)
+ {
+ if (collisionPair->m_algorithm)
+ {
+ manifoldArray.resize(0);
+ collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
+ for (int j=0;j<manifoldArray.size();j++)
+ {
+ btPersistentManifold* manifold = manifoldArray[j];
+ if (manifold->getNumContacts()>0)
+ return false;
+ }
+ }
+ }
+ }
+ return true;
+ }
+
+
+};
+
+///internal debugging variable. this value shouldn't be too high
+int gNumClampedCcdMotions=0;
+
+//#include "stdio.h"
+void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+ BT_PROFILE("integrateTransforms");
+ btTransform predictedTrans;
+ for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+ {
+ btRigidBody* body = m_nonStaticRigidBodies[i];
+ body->setHitFraction(1.f);
+
+ if (body->isActive() && (!body->isStaticOrKinematicObject()))
+ {
+ body->predictIntegratedTransform(timeStep, predictedTrans);
+ btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
+
+ if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
+ {
+ BT_PROFILE("CCD motion clamping");
+ if (body->getCollisionShape()->isConvex())
+ {
+ gNumClampedCcdMotions++;
+
+ btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+ //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+ btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(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))
+ {
+ body->setHitFraction(sweepResults.m_closestHitFraction);
+ body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
+ body->setHitFraction(0.f);
+// printf("clamped integration to hit fraction = %f\n",fraction);
+ }
+ }
+ }
+
+ body->proceedToTransform( predictedTrans);
+ }
+ }
+}
+
+
+
+
+
+void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+ BT_PROFILE("predictUnconstraintMotion");
+ for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+ {
+ btRigidBody* body = m_nonStaticRigidBodies[i];
+ if (!body->isStaticOrKinematicObject())
+ {
+ body->integrateVelocities( timeStep);
+ //damping
+ body->applyDamping(timeStep);
+
+ body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+ }
+ }
+}
+
+
+void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
+{
+ (void)timeStep;
+
+#ifndef BT_NO_PROFILE
+ CProfileManager::Reset();
+#endif //BT_NO_PROFILE
+
+}
+
+
+
+
+
+
+class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
+{
+ btIDebugDraw* m_debugDrawer;
+ btVector3 m_color;
+ btTransform m_worldTrans;
+
+public:
+
+ DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
+ m_debugDrawer(debugDrawer),
+ m_color(color),
+ m_worldTrans(worldTrans)
+ {
+ }
+
+ virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
+ {
+ processTriangle(triangle,partId,triangleIndex);
+ }
+
+ virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
+ {
+ (void)partId;
+ (void)triangleIndex;
+
+ btVector3 wv0,wv1,wv2;
+ wv0 = m_worldTrans*triangle[0];
+ wv1 = m_worldTrans*triangle[1];
+ wv2 = m_worldTrans*triangle[2];
+ m_debugDrawer->drawLine(wv0,wv1,m_color);
+ m_debugDrawer->drawLine(wv1,wv2,m_color);
+ m_debugDrawer->drawLine(wv2,wv0,m_color);
+ }
+};
+
+void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
+{
+ btVector3 start = transform.getOrigin();
+
+ const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
+ const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
+ const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
+
+ // XY
+ getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
+ getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
+ getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
+ getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
+
+ // XZ
+ getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
+ getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
+ getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
+ getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
+
+ // YZ
+ getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
+ getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
+ getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
+ getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
+}
+
+void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
+{
+ // Draw a small simplex at the center of the object
+ {
+ btVector3 start = worldTransform.getOrigin();
+ getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
+ getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
+ getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
+ }
+
+ if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+ {
+ const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
+ for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
+ {
+ btTransform childTrans = compoundShape->getChildTransform(i);
+ const btCollisionShape* colShape = compoundShape->getChildShape(i);
+ debugDrawObject(worldTransform*childTrans,colShape,color);
+ }
+
+ } else
+ {
+ switch (shape->getShapeType())
+ {
+
+ case SPHERE_SHAPE_PROXYTYPE:
+ {
+ const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
+ btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
+
+ debugDrawSphere(radius, worldTransform, color);
+ break;
+ }
+ case MULTI_SPHERE_SHAPE_PROXYTYPE:
+ {
+ const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
+
+ btTransform childTransform;
+ childTransform.setIdentity();
+
+ for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
+ {
+ childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
+ debugDrawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
+ }
+
+ break;
+ }
+ case CAPSULE_SHAPE_PROXYTYPE:
+ {
+ const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
+
+ btScalar radius = capsuleShape->getRadius();
+ btScalar halfHeight = capsuleShape->getHalfHeight();
+
+ int upAxis = capsuleShape->getUpAxis();
+
+
+ btVector3 capStart(0.f,0.f,0.f);
+ capStart[upAxis] = -halfHeight;
+
+ btVector3 capEnd(0.f,0.f,0.f);
+ capEnd[upAxis] = halfHeight;
+
+ // Draw the ends
+ {
+
+ btTransform childTransform = worldTransform;
+ childTransform.getOrigin() = worldTransform * capStart;
+ debugDrawSphere(radius, childTransform, color);
+ }
+
+ {
+ btTransform childTransform = worldTransform;
+ childTransform.getOrigin() = worldTransform * capEnd;
+ debugDrawSphere(radius, childTransform, color);
+ }
+
+ // Draw some additional lines
+ btVector3 start = worldTransform.getOrigin();
+
+
+ capStart[(upAxis+1)%3] = radius;
+ capEnd[(upAxis+1)%3] = radius;
+ getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+ capStart[(upAxis+1)%3] = -radius;
+ capEnd[(upAxis+1)%3] = -radius;
+ getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+ capStart[(upAxis+1)%3] = 0.f;
+ capEnd[(upAxis+1)%3] = 0.f;
+
+ capStart[(upAxis+2)%3] = radius;
+ capEnd[(upAxis+2)%3] = radius;
+ getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+ capStart[(upAxis+2)%3] = -radius;
+ capEnd[(upAxis+2)%3] = -radius;
+ getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+
+ break;
+ }
+ case CONE_SHAPE_PROXYTYPE:
+ {
+ const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
+ btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
+ btScalar height = coneShape->getHeight();//+coneShape->getMargin();
+ btVector3 start = worldTransform.getOrigin();
+
+ int upAxis= coneShape->getConeUpIndex();
+
+
+ btVector3 offsetHeight(0,0,0);
+ offsetHeight[upAxis] = height * btScalar(0.5);
+ btVector3 offsetRadius(0,0,0);
+ offsetRadius[(upAxis+1)%3] = radius;
+ btVector3 offset2Radius(0,0,0);
+ offset2Radius[(upAxis+2)%3] = radius;
+
+ getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+ getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+ getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
+ getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
+
+
+
+ break;
+
+ }
+ case CYLINDER_SHAPE_PROXYTYPE:
+ {
+ const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
+ int upAxis = cylinder->getUpAxis();
+ btScalar radius = cylinder->getRadius();
+ btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
+ btVector3 start = worldTransform.getOrigin();
+ btVector3 offsetHeight(0,0,0);
+ offsetHeight[upAxis] = halfHeight;
+ btVector3 offsetRadius(0,0,0);
+ offsetRadius[(upAxis+1)%3] = radius;
+ getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+ getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+ break;
+ }
+
+ case STATIC_PLANE_PROXYTYPE:
+ {
+ const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
+ btScalar planeConst = staticPlaneShape->getPlaneConstant();
+ const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
+ btVector3 planeOrigin = planeNormal * planeConst;
+ btVector3 vec0,vec1;
+ btPlaneSpace1(planeNormal,vec0,vec1);
+ btScalar vecLen = 100.f;
+ btVector3 pt0 = planeOrigin + vec0*vecLen;
+ btVector3 pt1 = planeOrigin - vec0*vecLen;
+ btVector3 pt2 = planeOrigin + vec1*vecLen;
+ btVector3 pt3 = planeOrigin - vec1*vecLen;
+ getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
+ getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
+ break;
+
+ }
+ default:
+ {
+
+ if (shape->isConcave())
+ {
+ btConcaveShape* concaveMesh = (btConcaveShape*) shape;
+
+ ///@todo pass camera, for some culling? no -> we are not a graphics lib
+ btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+
+ DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+ concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
+
+ }
+
+ if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
+ {
+ btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
+ //todo: pass camera for some culling
+ btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+ //DebugDrawcallback drawCallback;
+ DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+ convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+ }
+
+
+ /// for polyhedral shapes
+ if (shape->isPolyhedral())
+ {
+ btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
+
+ int i;
+ for (i=0;i<polyshape->getNumEdges();i++)
+ {
+ btVector3 a,b;
+ polyshape->getEdge(i,a,b);
+ btVector3 wa = worldTransform * a;
+ btVector3 wb = worldTransform * b;
+ getDebugDrawer()->drawLine(wa,wb,color);
+
+ }
+
+
+ }
+ }
+ }
+ }
+}
+
+
+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; i<nSegments; i++)
+ {
+ fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
+ btVector3 pCur = pCT->GetPointForAngle(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;
+}
+
+
+
+
+
+void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+{
+ if (m_ownsConstraintSolver)
+ {
+ btAlignedFree( m_constraintSolver);
+ }
+ m_ownsConstraintSolver = false;
+ m_constraintSolver = solver;
+}
+
+btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
+{
+ return m_constraintSolver;
+}
+
+
+int btDiscreteDynamicsWorld::getNumConstraints() const
+{
+ return int(m_constraints.size());
+}
+btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
+{
+ return m_constraints[index];
+}
+const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
+{
+ return m_constraints[index];
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btDispatcher.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btDispatcher.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btDispatcher.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btDispatcher.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,22 @@
+/*
+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 "BulletCollision/BroadphaseCollision/btDispatcher.h"
+
+btDispatcher::~btDispatcher()
+{
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btEmptyCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btEmptyCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btEmptyCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btEmptyCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,34 @@
+/*
+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 "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
+
+
+
+btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+ : btCollisionAlgorithm(ci)
+{
+}
+
+void btEmptyAlgorithm::processCollision (btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
+{
+}
+
+btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
+{
+ return btScalar(1.);
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btEmptyShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btEmptyShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btEmptyShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btEmptyShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btEmptyShape.h"
+
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+
+
+btEmptyShape::btEmptyShape() : btConcaveShape ()
+{
+ m_shapeType = EMPTY_SHAPE_PROXYTYPE;
+}
+
+
+btEmptyShape::~btEmptyShape()
+{
+}
+
+
+ ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ btVector3 margin(getMargin(),getMargin(),getMargin());
+
+ aabbMin = t.getOrigin() - margin;
+
+ aabbMax = t.getOrigin() + margin;
+
+}
+
+void btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) const
+{
+ btAssert(0);
+}
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactBvh.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactBvh.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactBvh.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactBvh.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,498 @@
+/*! \file gim_box_set.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.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 "BulletCollision/Gimpact/btGImpactBvh.h"
+#include "LinearMath/btQuickprof.h"
+
+#ifdef TRI_COLLISION_PROFILING
+
+btClock g_tree_clock;
+
+float g_accum_tree_collision_time = 0;
+int g_count_traversing = 0;
+
+
+void bt_begin_gim02_tree_time()
+{
+ g_tree_clock.reset();
+}
+
+void bt_end_gim02_tree_time()
+{
+ g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds();
+ g_count_traversing++;
+}
+
+//! Gets the average time in miliseconds of tree collisions
+float btGImpactBvh::getAverageTreeCollisionTime()
+{
+ if(g_count_traversing == 0) return 0;
+
+ float avgtime = g_accum_tree_collision_time;
+ avgtime /= (float)g_count_traversing;
+
+ g_accum_tree_collision_time = 0;
+ g_count_traversing = 0;
+ return avgtime;
+
+// float avgtime = g_count_traversing;
+// g_count_traversing = 0;
+// return avgtime;
+
+}
+
+#endif //TRI_COLLISION_PROFILING
+
+/////////////////////// btBvhTree /////////////////////////////////
+
+int btBvhTree::_calc_splitting_axis(
+ GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
+{
+
+ int i;
+
+ btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+ btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+ int numIndices = endIndex-startIndex;
+
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ means+=center;
+ }
+ means *= (btScalar(1.)/(btScalar)numIndices);
+
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ btVector3 diff2 = center-means;
+ diff2 = diff2 * diff2;
+ variance += diff2;
+ }
+ variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );
+
+ return variance.maxAxis();
+}
+
+
+int btBvhTree::_sort_and_calc_splitting_index(
+ GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
+ int endIndex, int splitAxis)
+{
+ int i;
+ int splitIndex =startIndex;
+ int numIndices = endIndex - startIndex;
+
+ // average of centers
+ btScalar splitValue = 0.0f;
+
+ btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ means+=center;
+ }
+ means *= (btScalar(1.)/(btScalar)numIndices);
+
+ splitValue = means[splitAxis];
+
+
+ //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ if (center[splitAxis] > splitValue)
+ {
+ //swap
+ primitive_boxes.swap(i,splitIndex);
+ //swapLeafNodes(i,splitIndex);
+ splitIndex++;
+ }
+ }
+
+ //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+ //otherwise the tree-building might fail due to stack-overflows in certain cases.
+ //unbalanced1 is unsafe: it can cause stack overflows
+ //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+ //unbalanced2 should work too: always use center (perfect balanced trees)
+ //bool unbalanced2 = true;
+
+ //this should be safe too:
+ int rangeBalancedIndices = numIndices/3;
+ bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+ if (unbalanced)
+ {
+ splitIndex = startIndex+ (numIndices>>1);
+ }
+
+ btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
+
+ return splitIndex;
+
+}
+
+
+void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
+{
+ int curIndex = m_num_nodes;
+ m_num_nodes++;
+
+ btAssert((endIndex-startIndex)>0);
+
+ if ((endIndex-startIndex)==1)
+ {
+ //We have a leaf node
+ setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
+ m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
+
+ return;
+ }
+ //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+ //split axis
+ int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+ splitIndex = _sort_and_calc_splitting_index(
+ primitive_boxes,startIndex,endIndex,
+ splitIndex//split axis
+ );
+
+
+ //calc this node bounding box
+
+ btAABB node_bound;
+ node_bound.invalidate();
+
+ for (int i=startIndex;i<endIndex;i++)
+ {
+ node_bound.merge(primitive_boxes[i].m_bound);
+ }
+
+ setNodeBound(curIndex,node_bound);
+
+
+ //build left branch
+ _build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+
+ //build right branch
+ _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+ m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
+
+
+}
+
+//! stackless build tree
+void btBvhTree::build_tree(
+ GIM_BVH_DATA_ARRAY & primitive_boxes)
+{
+ // initialize node count to 0
+ m_num_nodes = 0;
+ // allocate nodes
+ m_node_array.resize(primitive_boxes.size()*2);
+
+ _build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+
+////////////////////////////////////class btGImpactBvh
+
+void btGImpactBvh::refit()
+{
+ int nodecount = getNodeCount();
+ while(nodecount--)
+ {
+ if(isLeafNode(nodecount))
+ {
+ btAABB leafbox;
+ m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
+ setNodeBound(nodecount,leafbox);
+ }
+ else
+ {
+ //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
+ //get left bound
+ btAABB bound;
+ bound.invalidate();
+
+ btAABB temp_box;
+
+ int child_node = getLeftNode(nodecount);
+ if(child_node)
+ {
+ getNodeBound(child_node,temp_box);
+ bound.merge(temp_box);
+ }
+
+ child_node = getRightNode(nodecount);
+ if(child_node)
+ {
+ getNodeBound(child_node,temp_box);
+ bound.merge(temp_box);
+ }
+
+ setNodeBound(nodecount,bound);
+ }
+ }
+}
+
+//! this rebuild the entire set
+void btGImpactBvh::buildSet()
+{
+ //obtain primitive boxes
+ GIM_BVH_DATA_ARRAY primitive_boxes;
+ primitive_boxes.resize(m_primitive_manager->get_primitive_count());
+
+ for (int i = 0;i<primitive_boxes.size() ;i++ )
+ {
+ m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
+ primitive_boxes[i].m_data = i;
+ }
+
+ m_box_tree.build_tree(primitive_boxes);
+}
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
+{
+ int curIndex = 0;
+ int numNodes = getNodeCount();
+
+ while (curIndex < numNodes)
+ {
+ btAABB bound;
+ getNodeBound(curIndex,bound);
+
+ //catch bugs in tree data
+
+ bool aabbOverlap = bound.has_collision(box);
+ bool isleafnode = isLeafNode(curIndex);
+
+ if (isleafnode && aabbOverlap)
+ {
+ collided_results.push_back(getNodeData(curIndex));
+ }
+
+ if (aabbOverlap || isleafnode)
+ {
+ //next subnode
+ curIndex++;
+ }
+ else
+ {
+ //skip node
+ curIndex+= getEscapeNodeIndex(curIndex);
+ }
+ }
+ if(collided_results.size()>0) return true;
+ return false;
+}
+
+
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactBvh::rayQuery(
+ const btVector3 & ray_dir,const btVector3 & ray_origin ,
+ btAlignedObjectArray<int> & collided_results) const
+{
+ int curIndex = 0;
+ int numNodes = getNodeCount();
+
+ while (curIndex < numNodes)
+ {
+ btAABB bound;
+ getNodeBound(curIndex,bound);
+
+ //catch bugs in tree data
+
+ bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+ bool isleafnode = isLeafNode(curIndex);
+
+ if (isleafnode && aabbOverlap)
+ {
+ collided_results.push_back(getNodeData( curIndex));
+ }
+
+ if (aabbOverlap || isleafnode)
+ {
+ //next subnode
+ curIndex++;
+ }
+ else
+ {
+ //skip node
+ curIndex+= getEscapeNodeIndex(curIndex);
+ }
+ }
+ if(collided_results.size()>0) return true;
+ return false;
+}
+
+
+SIMD_FORCE_INLINE bool _node_collision(
+ btGImpactBvh * boxset0, btGImpactBvh * boxset1,
+ const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+ int node0 ,int node1, bool complete_primitive_tests)
+{
+ btAABB box0;
+ boxset0->getNodeBound(node0,box0);
+ btAABB box1;
+ boxset1->getNodeBound(node1,box1);
+
+ return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
+// box1.appy_transform_trans_cache(trans_cache_1to0);
+// return box0.has_collision(box1);
+
+}
+
+
+//stackless recursive collision routine
+static void _find_collision_pairs_recursive(
+ btGImpactBvh * boxset0, btGImpactBvh * boxset1,
+ btPairSet * collision_pairs,
+ const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+ int node0, int node1, bool complete_primitive_tests)
+{
+
+
+
+ if( _node_collision(
+ boxset0,boxset1,trans_cache_1to0,
+ node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
+
+ if(boxset0->isLeafNode(node0))
+ {
+ if(boxset1->isLeafNode(node1))
+ {
+ // collision result
+ collision_pairs->push_pair(
+ boxset0->getNodeData(node0),boxset1->getNodeData(node1));
+ return;
+ }
+ else
+ {
+
+ //collide left recursive
+
+ _find_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ node0,boxset1->getLeftNode(node1),false);
+
+ //collide right recursive
+ _find_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ node0,boxset1->getRightNode(node1),false);
+
+
+ }
+ }
+ else
+ {
+ if(boxset1->isLeafNode(node1))
+ {
+
+ //collide left recursive
+ _find_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getLeftNode(node0),node1,false);
+
+
+ //collide right recursive
+
+ _find_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getRightNode(node0),node1,false);
+
+
+ }
+ else
+ {
+ //collide left0 left1
+
+
+
+ _find_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
+
+ //collide left0 right1
+
+ _find_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
+
+
+ //collide right0 left1
+
+ _find_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
+
+ //collide right0 right1
+
+ _find_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
+
+ }// else if node1 is not a leaf
+ }// else if node0 is not a leaf
+}
+
+
+void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0,
+ btGImpactBvh * boxset1, const btTransform & trans1,
+ btPairSet & collision_pairs)
+{
+
+ if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
+
+ BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
+
+ trans_cache_1to0.calc_from_homogenic(trans0,trans1);
+
+#ifdef TRI_COLLISION_PROFILING
+ bt_begin_gim02_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+ _find_collision_pairs_recursive(
+ boxset0,boxset1,
+ &collision_pairs,trans_cache_1to0,0,0,true);
+#ifdef TRI_COLLISION_PROFILING
+ bt_end_gim02_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,902 @@
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.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.
+*/
+/*
+Author: Francisco Len Nßjera
+Concave-Concave Collision
+
+*/
+
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
+#include "BulletCollision/Gimpact/btContactProcessing.h"
+#include "LinearMath/btQuickprof.h"
+
+
+//! Class for accessing the plane equation
+class btPlaneShape : public btStaticPlaneShape
+{
+public:
+
+ btPlaneShape(const btVector3& v, float f)
+ :btStaticPlaneShape(v,f)
+ {
+ }
+
+ void get_plane_equation(btVector4 &equation)
+ {
+ equation[0] = m_planeNormal[0];
+ equation[1] = m_planeNormal[1];
+ equation[2] = m_planeNormal[2];
+ equation[3] = m_planeConstant;
+ }
+
+
+ void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation)
+ {
+ equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal);
+ equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal);
+ equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal);
+ equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant;
+ }
+};
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+#ifdef TRI_COLLISION_PROFILING
+
+btClock g_triangle_clock;
+
+float g_accum_triangle_collision_time = 0;
+int g_count_triangle_collision = 0;
+
+void bt_begin_gim02_tri_time()
+{
+ g_triangle_clock.reset();
+}
+
+void bt_end_gim02_tri_time()
+{
+ g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds();
+ g_count_triangle_collision++;
+}
+#endif //TRI_COLLISION_PROFILING
+//! Retrieving shapes shapes
+/*!
+Declared here due of insuficent space on Pool allocators
+*/
+//!@{
+class GIM_ShapeRetriever
+{
+public:
+ btGImpactShapeInterface * m_gim_shape;
+ btTriangleShapeEx m_trishape;
+ btTetrahedronShapeEx m_tetrashape;
+
+public:
+ class ChildShapeRetriever
+ {
+ public:
+ GIM_ShapeRetriever * m_parent;
+ virtual btCollisionShape * getChildShape(int index)
+ {
+ return m_parent->m_gim_shape->getChildShape(index);
+ }
+ virtual ~ChildShapeRetriever() {}
+ };
+
+ class TriangleShapeRetriever:public ChildShapeRetriever
+ {
+ public:
+
+ virtual btCollisionShape * getChildShape(int index)
+ {
+ m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape);
+ return &m_parent->m_trishape;
+ }
+ virtual ~TriangleShapeRetriever() {}
+ };
+
+ class TetraShapeRetriever:public ChildShapeRetriever
+ {
+ public:
+
+ virtual btCollisionShape * getChildShape(int index)
+ {
+ m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape);
+ return &m_parent->m_tetrashape;
+ }
+ };
+public:
+ ChildShapeRetriever m_child_retriever;
+ TriangleShapeRetriever m_tri_retriever;
+ TetraShapeRetriever m_tetra_retriever;
+ ChildShapeRetriever * m_current_retriever;
+
+ GIM_ShapeRetriever(btGImpactShapeInterface * gim_shape)
+ {
+ m_gim_shape = gim_shape;
+ //select retriever
+ if(m_gim_shape->needsRetrieveTriangles())
+ {
+ m_current_retriever = &m_tri_retriever;
+ }
+ else if(m_gim_shape->needsRetrieveTetrahedrons())
+ {
+ m_current_retriever = &m_tetra_retriever;
+ }
+ else
+ {
+ m_current_retriever = &m_child_retriever;
+ }
+
+ m_current_retriever->m_parent = this;
+ }
+
+ btCollisionShape * getChildShape(int index)
+ {
+ return m_current_retriever->getChildShape(index);
+ }
+
+
+};
+
+
+
+//!@}
+
+
+#ifdef TRI_COLLISION_PROFILING
+
+//! Gets the average time in miliseconds of tree collisions
+float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime()
+{
+ return btGImpactBoxSet::getAverageTreeCollisionTime();
+
+}
+
+//! Gets the average time in miliseconds of triangle collisions
+float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime()
+{
+ if(g_count_triangle_collision == 0) return 0;
+
+ float avgtime = g_accum_triangle_collision_time;
+ avgtime /= (float)g_count_triangle_collision;
+
+ g_accum_triangle_collision_time = 0;
+ g_count_triangle_collision = 0;
+
+ return avgtime;
+}
+
+#endif //TRI_COLLISION_PROFILING
+
+
+
+btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+: btActivatingCollisionAlgorithm(ci,body0,body1)
+{
+ m_manifoldPtr = NULL;
+ m_convex_algorithm = NULL;
+}
+
+btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm()
+{
+ clearCache();
+}
+
+
+
+
+
+void btGImpactCollisionAlgorithm::addContactPoint(btCollisionObject * body0,
+ btCollisionObject * body1,
+ const btVector3 & point,
+ const btVector3 & normal,
+ btScalar distance)
+{
+ m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
+ m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
+ checkManifold(body0,body1);
+ m_resultOut->addContactPoint(normal,point,distance);
+}
+
+
+void btGImpactCollisionAlgorithm::shape_vs_shape_collision(
+ btCollisionObject * body0,
+ btCollisionObject * body1,
+ btCollisionShape * shape0,
+ btCollisionShape * shape1)
+{
+
+ btCollisionShape* tmpShape0 = body0->getCollisionShape();
+ btCollisionShape* tmpShape1 = body1->getCollisionShape();
+
+ body0->internalSetTemporaryCollisionShape(shape0);
+ body1->internalSetTemporaryCollisionShape(shape1);
+
+ {
+ btCollisionAlgorithm* algor = newAlgorithm(body0,body1);
+ // post : checkManifold is called
+
+ m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
+ m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
+
+ algor->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
+
+ algor->~btCollisionAlgorithm();
+ m_dispatcher->freeCollisionAlgorithm(algor);
+ }
+
+ body0->internalSetTemporaryCollisionShape(tmpShape0);
+ body1->internalSetTemporaryCollisionShape(tmpShape1);
+}
+
+void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
+ btCollisionObject * body0,
+ btCollisionObject * body1,
+ btCollisionShape * shape0,
+ btCollisionShape * shape1)
+{
+
+ btCollisionShape* tmpShape0 = body0->getCollisionShape();
+ btCollisionShape* tmpShape1 = body1->getCollisionShape();
+
+ body0->internalSetTemporaryCollisionShape(shape0);
+ body1->internalSetTemporaryCollisionShape(shape1);
+
+
+ m_resultOut->setShapeIdentifiersA(m_part0,m_triface0);
+ m_resultOut->setShapeIdentifiersB(m_part1,m_triface1);
+
+ checkConvexAlgorithm(body0,body1);
+ m_convex_algorithm->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
+
+ body0->internalSetTemporaryCollisionShape(tmpShape0);
+ body1->internalSetTemporaryCollisionShape(tmpShape1);
+
+}
+
+
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
+ const btTransform & trans0,
+ const btTransform & trans1,
+ btGImpactShapeInterface * shape0,
+ btGImpactShapeInterface * shape1,btPairSet & pairset)
+{
+ if(shape0->hasBoxSet() && shape1->hasBoxSet())
+ {
+ btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset);
+ }
+ else
+ {
+ btAABB boxshape0;
+ btAABB boxshape1;
+ int i = shape0->getNumChildShapes();
+
+ while(i--)
+ {
+ shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
+
+ int j = shape1->getNumChildShapes();
+ while(j--)
+ {
+ shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max);
+
+ if(boxshape1.has_collision(boxshape0))
+ {
+ pairset.push_pair(i,j);
+ }
+ }
+ }
+ }
+
+
+}
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
+ const btTransform & trans0,
+ const btTransform & trans1,
+ btGImpactShapeInterface * shape0,
+ btCollisionShape * shape1,
+ btAlignedObjectArray<int> & collided_primitives)
+{
+
+ btAABB boxshape;
+
+
+ if(shape0->hasBoxSet())
+ {
+ btTransform trans1to0 = trans0.inverse();
+ trans1to0 *= trans1;
+
+ shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
+
+ shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
+ }
+ else
+ {
+ shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);
+
+ btAABB boxshape0;
+ int i = shape0->getNumChildShapes();
+
+ while(i--)
+ {
+ shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
+
+ if(boxshape.has_collision(boxshape0))
+ {
+ collided_primitives.push_back(i);
+ }
+ }
+
+ }
+
+}
+
+
+void btGImpactCollisionAlgorithm::collide_gjk_triangles(btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactMeshShapePart * shape0,
+ btGImpactMeshShapePart * shape1,
+ const int * pairs, int pair_count)
+{
+ btTriangleShapeEx tri0;
+ btTriangleShapeEx tri1;
+
+ shape0->lockChildShapes();
+ shape1->lockChildShapes();
+
+ const int * pair_pointer = pairs;
+
+ while(pair_count--)
+ {
+
+ m_triface0 = *(pair_pointer);
+ m_triface1 = *(pair_pointer+1);
+ pair_pointer+=2;
+
+
+
+ shape0->getBulletTriangle(m_triface0,tri0);
+ shape1->getBulletTriangle(m_triface1,tri1);
+
+
+ //collide two convex shapes
+ if(tri0.overlap_test_conservative(tri1))
+ {
+ convex_vs_convex_collision(body0,body1,&tri0,&tri1);
+ }
+
+ }
+
+ shape0->unlockChildShapes();
+ shape1->unlockChildShapes();
+}
+
+void btGImpactCollisionAlgorithm::collide_sat_triangles(btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactMeshShapePart * shape0,
+ btGImpactMeshShapePart * shape1,
+ const int * pairs, int pair_count)
+{
+ btTransform orgtrans0 = body0->getWorldTransform();
+ btTransform orgtrans1 = body1->getWorldTransform();
+
+ btPrimitiveTriangle ptri0;
+ btPrimitiveTriangle ptri1;
+ GIM_TRIANGLE_CONTACT contact_data;
+
+ shape0->lockChildShapes();
+ shape1->lockChildShapes();
+
+ const int * pair_pointer = pairs;
+
+ while(pair_count--)
+ {
+
+ m_triface0 = *(pair_pointer);
+ m_triface1 = *(pair_pointer+1);
+ pair_pointer+=2;
+
+
+ shape0->getPrimitiveTriangle(m_triface0,ptri0);
+ shape1->getPrimitiveTriangle(m_triface1,ptri1);
+
+ #ifdef TRI_COLLISION_PROFILING
+ bt_begin_gim02_tri_time();
+ #endif
+
+ ptri0.applyTransform(orgtrans0);
+ ptri1.applyTransform(orgtrans1);
+
+
+ //build planes
+ ptri0.buildTriPlane();
+ ptri1.buildTriPlane();
+ // test conservative
+
+
+
+ if(ptri0.overlap_test_conservative(ptri1))
+ {
+ if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data))
+ {
+
+ int j = contact_data.m_point_count;
+ while(j--)
+ {
+
+ addContactPoint(body0, body1,
+ contact_data.m_points[j],
+ contact_data.m_separating_normal,
+ -contact_data.m_penetration_depth);
+ }
+ }
+ }
+
+ #ifdef TRI_COLLISION_PROFILING
+ bt_end_gim02_tri_time();
+ #endif
+
+ }
+
+ shape0->unlockChildShapes();
+ shape1->unlockChildShapes();
+
+}
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
+ btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactShapeInterface * shape0,
+ btGImpactShapeInterface * shape1)
+{
+
+ if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+ {
+ btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0);
+ m_part0 = meshshape0->getMeshPartCount();
+
+ while(m_part0--)
+ {
+ gimpact_vs_gimpact(body0,body1,meshshape0->getMeshPart(m_part0),shape1);
+ }
+
+ return;
+ }
+
+ if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+ {
+ btGImpactMeshShape * meshshape1 = static_cast<btGImpactMeshShape *>(shape1);
+ m_part1 = meshshape1->getMeshPartCount();
+
+ while(m_part1--)
+ {
+
+ gimpact_vs_gimpact(body0,body1,shape0,meshshape1->getMeshPart(m_part1));
+
+ }
+
+ return;
+ }
+
+
+ btTransform orgtrans0 = body0->getWorldTransform();
+ btTransform orgtrans1 = body1->getWorldTransform();
+
+ btPairSet pairset;
+
+ gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
+
+ if(pairset.size()== 0) return;
+
+ if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
+ shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART)
+ {
+ btGImpactMeshShapePart * shapepart0 = static_cast<btGImpactMeshShapePart * >(shape0);
+ btGImpactMeshShapePart * shapepart1 = static_cast<btGImpactMeshShapePart * >(shape1);
+ //specialized function
+ #ifdef BULLET_TRIANGLE_COLLISION
+ collide_gjk_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
+ #else
+ collide_sat_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
+ #endif
+
+ return;
+ }
+
+ //general function
+
+ shape0->lockChildShapes();
+ shape1->lockChildShapes();
+
+ GIM_ShapeRetriever retriever0(shape0);
+ GIM_ShapeRetriever retriever1(shape1);
+
+ bool child_has_transform0 = shape0->childrenHasTransform();
+ bool child_has_transform1 = shape1->childrenHasTransform();
+
+ int i = pairset.size();
+ while(i--)
+ {
+ GIM_PAIR * pair = &pairset[i];
+ m_triface0 = pair->m_index1;
+ m_triface1 = pair->m_index2;
+ btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0);
+ btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1);
+
+ if(child_has_transform0)
+ {
+ body0->setWorldTransform(orgtrans0*shape0->getChildTransform(m_triface0));
+ }
+
+ if(child_has_transform1)
+ {
+ body1->setWorldTransform(orgtrans1*shape1->getChildTransform(m_triface1));
+ }
+
+ //collide two convex shapes
+ convex_vs_convex_collision(body0,body1,colshape0,colshape1);
+
+
+ if(child_has_transform0)
+ {
+ body0->setWorldTransform(orgtrans0);
+ }
+
+ if(child_has_transform1)
+ {
+ body1->setWorldTransform(orgtrans1);
+ }
+
+ }
+
+ shape0->unlockChildShapes();
+ shape1->unlockChildShapes();
+}
+
+void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactShapeInterface * shape0,
+ btCollisionShape * shape1,bool swapped)
+{
+ if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+ {
+ btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0);
+ int& part = swapped ? m_part1 : m_part0;
+ part = meshshape0->getMeshPartCount();
+
+ while(part--)
+ {
+
+ gimpact_vs_shape(body0,
+ body1,
+ meshshape0->getMeshPart(part),
+ shape1,swapped);
+
+ }
+
+ return;
+ }
+
+ #ifdef GIMPACT_VS_PLANE_COLLISION
+ if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
+ shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
+ {
+ btGImpactMeshShapePart * shapepart = static_cast<btGImpactMeshShapePart *>(shape0);
+ btStaticPlaneShape * planeshape = static_cast<btStaticPlaneShape * >(shape1);
+ gimpacttrimeshpart_vs_plane_collision(body0,body1,shapepart,planeshape,swapped);
+ return;
+ }
+
+ #endif
+
+
+
+ if(shape1->isCompound())
+ {
+ btCompoundShape * compoundshape = static_cast<btCompoundShape *>(shape1);
+ gimpact_vs_compoundshape(body0,body1,shape0,compoundshape,swapped);
+ return;
+ }
+ else if(shape1->isConcave())
+ {
+ btConcaveShape * concaveshape = static_cast<btConcaveShape *>(shape1);
+ gimpact_vs_concave(body0,body1,shape0,concaveshape,swapped);
+ return;
+ }
+
+
+ btTransform orgtrans0 = body0->getWorldTransform();
+
+ btTransform orgtrans1 = body1->getWorldTransform();
+
+ btAlignedObjectArray<int> collided_results;
+
+ gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results);
+
+ if(collided_results.size() == 0) return;
+
+
+ shape0->lockChildShapes();
+
+ GIM_ShapeRetriever retriever0(shape0);
+
+
+ bool child_has_transform0 = shape0->childrenHasTransform();
+
+
+ int i = collided_results.size();
+
+ while(i--)
+ {
+ int child_index = collided_results[i];
+ if(swapped)
+ m_triface1 = child_index;
+ else
+ m_triface0 = child_index;
+
+ btCollisionShape * colshape0 = retriever0.getChildShape(child_index);
+
+ if(child_has_transform0)
+ {
+ body0->setWorldTransform(orgtrans0*shape0->getChildTransform(child_index));
+ }
+
+ //collide two shapes
+ if(swapped)
+ {
+ shape_vs_shape_collision(body1,body0,shape1,colshape0);
+ }
+ else
+ {
+ shape_vs_shape_collision(body0,body1,colshape0,shape1);
+ }
+
+ //restore transforms
+ if(child_has_transform0)
+ {
+ body0->setWorldTransform(orgtrans0);
+ }
+
+ }
+
+ shape0->unlockChildShapes();
+
+}
+
+void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactShapeInterface * shape0,
+ btCompoundShape * shape1,bool swapped)
+{
+ btTransform orgtrans1 = body1->getWorldTransform();
+
+ int i = shape1->getNumChildShapes();
+ while(i--)
+ {
+
+ btCollisionShape * colshape1 = shape1->getChildShape(i);
+ btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i);
+
+ body1->setWorldTransform(childtrans1);
+
+ //collide child shape
+ gimpact_vs_shape(body0, body1,
+ shape0,colshape1,swapped);
+
+
+ //restore transforms
+ body1->setWorldTransform(orgtrans1);
+ }
+}
+
+void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
+ btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactMeshShapePart * shape0,
+ btStaticPlaneShape * shape1,bool swapped)
+{
+
+
+ btTransform orgtrans0 = body0->getWorldTransform();
+ btTransform orgtrans1 = body1->getWorldTransform();
+
+ btPlaneShape * planeshape = static_cast<btPlaneShape *>(shape1);
+ btVector4 plane;
+ planeshape->get_plane_equation_transformed(orgtrans1,plane);
+
+ //test box against plane
+
+ btAABB tribox;
+ shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max);
+ tribox.increment_margin(planeshape->getMargin());
+
+ if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return;
+
+ shape0->lockChildShapes();
+
+ btScalar margin = shape0->getMargin() + planeshape->getMargin();
+
+ btVector3 vertex;
+ int vi = shape0->getVertexCount();
+ while(vi--)
+ {
+ shape0->getVertex(vi,vertex);
+ vertex = orgtrans0(vertex);
+
+ btScalar distance = vertex.dot(plane) - plane[3] - margin;
+
+ if(distance<0.0)//add contact
+ {
+ if(swapped)
+ {
+ addContactPoint(body1, body0,
+ vertex,
+ -plane,
+ distance);
+ }
+ else
+ {
+ addContactPoint(body0, body1,
+ vertex,
+ plane,
+ distance);
+ }
+ }
+ }
+
+ shape0->unlockChildShapes();
+}
+
+
+
+
+class btGImpactTriangleCallback: public btTriangleCallback
+{
+public:
+ btGImpactCollisionAlgorithm * algorithm;
+ btCollisionObject * body0;
+ btCollisionObject * body1;
+ btGImpactShapeInterface * gimpactshape0;
+ bool swapped;
+ btScalar margin;
+
+ virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+ {
+ btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]);
+ tri1.setMargin(margin);
+ if(swapped)
+ {
+ algorithm->setPart0(partId);
+ algorithm->setFace0(triangleIndex);
+ }
+ else
+ {
+ algorithm->setPart1(partId);
+ algorithm->setFace1(triangleIndex);
+ }
+ algorithm->gimpact_vs_shape(
+ body0,body1,gimpactshape0,&tri1,swapped);
+ }
+};
+
+
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_concave(
+ btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactShapeInterface * shape0,
+ btConcaveShape * shape1,bool swapped)
+{
+ //create the callback
+ btGImpactTriangleCallback tricallback;
+ tricallback.algorithm = this;
+ tricallback.body0 = body0;
+ tricallback.body1 = body1;
+ tricallback.gimpactshape0 = shape0;
+ tricallback.swapped = swapped;
+ tricallback.margin = shape1->getMargin();
+
+ //getting the trimesh AABB
+ btTransform gimpactInConcaveSpace;
+
+ gimpactInConcaveSpace = body1->getWorldTransform().inverse() * body0->getWorldTransform();
+
+ btVector3 minAABB,maxAABB;
+ shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB);
+
+ shape1->processAllTriangles(&tricallback,minAABB,maxAABB);
+
+}
+
+
+
+void btGImpactCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ clearCache();
+
+ m_resultOut = resultOut;
+ m_dispatchInfo = &dispatchInfo;
+ btGImpactShapeInterface * gimpactshape0;
+ btGImpactShapeInterface * gimpactshape1;
+
+ if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
+ {
+ gimpactshape0 = static_cast<btGImpactShapeInterface *>(body0->getCollisionShape());
+
+ if( body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
+ {
+ gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
+
+ gimpact_vs_gimpact(body0,body1,gimpactshape0,gimpactshape1);
+ }
+ else
+ {
+ gimpact_vs_shape(body0,body1,gimpactshape0,body1->getCollisionShape(),false);
+ }
+
+ }
+ else if (body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
+ {
+ gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
+
+ gimpact_vs_shape(body1,body0,gimpactshape1,body0->getCollisionShape(),true);
+ }
+}
+
+
+btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ return 1.f;
+
+}
+
+///////////////////////////////////// REGISTERING ALGORITHM //////////////////////////////////////////////
+
+btGImpactCollisionAlgorithm::CreateFunc g_gimpact_cf;
+
+//! Use this function for register the algorithm externally
+void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher)
+{
+
+ int i;
+
+ for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
+ {
+ dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&g_gimpact_cf);
+ }
+
+ for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
+ {
+ dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&g_gimpact_cf);
+ }
+
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactQuantizedBvh.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactQuantizedBvh.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactQuantizedBvh.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactQuantizedBvh.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,528 @@
+/*! \file gim_box_set.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.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 "BulletCollision/Gimpact/btGImpactQuantizedBvh.h"
+#include "LinearMath/btQuickprof.h"
+
+#ifdef TRI_COLLISION_PROFILING
+btClock g_q_tree_clock;
+
+
+float g_q_accum_tree_collision_time = 0;
+int g_q_count_traversing = 0;
+
+
+void bt_begin_gim02_q_tree_time()
+{
+ g_q_tree_clock.reset();
+}
+
+void bt_end_gim02_q_tree_time()
+{
+ g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds();
+ g_q_count_traversing++;
+}
+
+
+//! Gets the average time in miliseconds of tree collisions
+float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
+{
+ if(g_q_count_traversing == 0) return 0;
+
+ float avgtime = g_q_accum_tree_collision_time;
+ avgtime /= (float)g_q_count_traversing;
+
+ g_q_accum_tree_collision_time = 0;
+ g_q_count_traversing = 0;
+ return avgtime;
+
+// float avgtime = g_q_count_traversing;
+// g_q_count_traversing = 0;
+// return avgtime;
+
+}
+
+#endif //TRI_COLLISION_PROFILING
+
+/////////////////////// btQuantizedBvhTree /////////////////////////////////
+
+void btQuantizedBvhTree::calc_quantization(
+ GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin)
+{
+ //calc globa box
+ btAABB global_bound;
+ global_bound.invalidate();
+
+ for (int i=0;i<primitive_boxes.size() ;i++ )
+ {
+ global_bound.merge(primitive_boxes[i].m_bound);
+ }
+
+ bt_calc_quantization_parameters(
+ m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin);
+
+}
+
+
+
+int btQuantizedBvhTree::_calc_splitting_axis(
+ GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
+{
+
+ int i;
+
+ btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+ btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+ int numIndices = endIndex-startIndex;
+
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ means+=center;
+ }
+ means *= (btScalar(1.)/(btScalar)numIndices);
+
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ btVector3 diff2 = center-means;
+ diff2 = diff2 * diff2;
+ variance += diff2;
+ }
+ variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );
+
+ return variance.maxAxis();
+}
+
+
+int btQuantizedBvhTree::_sort_and_calc_splitting_index(
+ GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
+ int endIndex, int splitAxis)
+{
+ int i;
+ int splitIndex =startIndex;
+ int numIndices = endIndex - startIndex;
+
+ // average of centers
+ btScalar splitValue = 0.0f;
+
+ btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ means+=center;
+ }
+ means *= (btScalar(1.)/(btScalar)numIndices);
+
+ splitValue = means[splitAxis];
+
+
+ //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ if (center[splitAxis] > splitValue)
+ {
+ //swap
+ primitive_boxes.swap(i,splitIndex);
+ //swapLeafNodes(i,splitIndex);
+ splitIndex++;
+ }
+ }
+
+ //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+ //otherwise the tree-building might fail due to stack-overflows in certain cases.
+ //unbalanced1 is unsafe: it can cause stack overflows
+ //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+ //unbalanced2 should work too: always use center (perfect balanced trees)
+ //bool unbalanced2 = true;
+
+ //this should be safe too:
+ int rangeBalancedIndices = numIndices/3;
+ bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+ if (unbalanced)
+ {
+ splitIndex = startIndex+ (numIndices>>1);
+ }
+
+ btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
+
+ return splitIndex;
+
+}
+
+
+void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
+{
+ int curIndex = m_num_nodes;
+ m_num_nodes++;
+
+ btAssert((endIndex-startIndex)>0);
+
+ if ((endIndex-startIndex)==1)
+ {
+ //We have a leaf node
+ setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
+ m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
+
+ return;
+ }
+ //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+ //split axis
+ int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+ splitIndex = _sort_and_calc_splitting_index(
+ primitive_boxes,startIndex,endIndex,
+ splitIndex//split axis
+ );
+
+
+ //calc this node bounding box
+
+ btAABB node_bound;
+ node_bound.invalidate();
+
+ for (int i=startIndex;i<endIndex;i++)
+ {
+ node_bound.merge(primitive_boxes[i].m_bound);
+ }
+
+ setNodeBound(curIndex,node_bound);
+
+
+ //build left branch
+ _build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+
+ //build right branch
+ _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+ m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
+
+
+}
+
+//! stackless build tree
+void btQuantizedBvhTree::build_tree(
+ GIM_BVH_DATA_ARRAY & primitive_boxes)
+{
+ calc_quantization(primitive_boxes);
+ // initialize node count to 0
+ m_num_nodes = 0;
+ // allocate nodes
+ m_node_array.resize(primitive_boxes.size()*2);
+
+ _build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+
+////////////////////////////////////class btGImpactQuantizedBvh
+
+void btGImpactQuantizedBvh::refit()
+{
+ int nodecount = getNodeCount();
+ while(nodecount--)
+ {
+ if(isLeafNode(nodecount))
+ {
+ btAABB leafbox;
+ m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
+ setNodeBound(nodecount,leafbox);
+ }
+ else
+ {
+ //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
+ //get left bound
+ btAABB bound;
+ bound.invalidate();
+
+ btAABB temp_box;
+
+ int child_node = getLeftNode(nodecount);
+ if(child_node)
+ {
+ getNodeBound(child_node,temp_box);
+ bound.merge(temp_box);
+ }
+
+ child_node = getRightNode(nodecount);
+ if(child_node)
+ {
+ getNodeBound(child_node,temp_box);
+ bound.merge(temp_box);
+ }
+
+ setNodeBound(nodecount,bound);
+ }
+ }
+}
+
+//! this rebuild the entire set
+void btGImpactQuantizedBvh::buildSet()
+{
+ //obtain primitive boxes
+ GIM_BVH_DATA_ARRAY primitive_boxes;
+ primitive_boxes.resize(m_primitive_manager->get_primitive_count());
+
+ for (int i = 0;i<primitive_boxes.size() ;i++ )
+ {
+ m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
+ primitive_boxes[i].m_data = i;
+ }
+
+ m_box_tree.build_tree(primitive_boxes);
+}
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
+{
+ int curIndex = 0;
+ int numNodes = getNodeCount();
+
+ //quantize box
+
+ unsigned short quantizedMin[3];
+ unsigned short quantizedMax[3];
+
+ m_box_tree.quantizePoint(quantizedMin,box.m_min);
+ m_box_tree.quantizePoint(quantizedMax,box.m_max);
+
+
+ while (curIndex < numNodes)
+ {
+
+ //catch bugs in tree data
+
+ bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax);
+ bool isleafnode = isLeafNode(curIndex);
+
+ if (isleafnode && aabbOverlap)
+ {
+ collided_results.push_back(getNodeData(curIndex));
+ }
+
+ if (aabbOverlap || isleafnode)
+ {
+ //next subnode
+ curIndex++;
+ }
+ else
+ {
+ //skip node
+ curIndex+= getEscapeNodeIndex(curIndex);
+ }
+ }
+ if(collided_results.size()>0) return true;
+ return false;
+}
+
+
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactQuantizedBvh::rayQuery(
+ const btVector3 & ray_dir,const btVector3 & ray_origin ,
+ btAlignedObjectArray<int> & collided_results) const
+{
+ int curIndex = 0;
+ int numNodes = getNodeCount();
+
+ while (curIndex < numNodes)
+ {
+ btAABB bound;
+ getNodeBound(curIndex,bound);
+
+ //catch bugs in tree data
+
+ bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+ bool isleafnode = isLeafNode(curIndex);
+
+ if (isleafnode && aabbOverlap)
+ {
+ collided_results.push_back(getNodeData( curIndex));
+ }
+
+ if (aabbOverlap || isleafnode)
+ {
+ //next subnode
+ curIndex++;
+ }
+ else
+ {
+ //skip node
+ curIndex+= getEscapeNodeIndex(curIndex);
+ }
+ }
+ if(collided_results.size()>0) return true;
+ return false;
+}
+
+
+SIMD_FORCE_INLINE bool _quantized_node_collision(
+ btGImpactQuantizedBvh * boxset0, btGImpactQuantizedBvh * boxset1,
+ const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+ int node0 ,int node1, bool complete_primitive_tests)
+{
+ btAABB box0;
+ boxset0->getNodeBound(node0,box0);
+ btAABB box1;
+ boxset1->getNodeBound(node1,box1);
+
+ return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
+// box1.appy_transform_trans_cache(trans_cache_1to0);
+// return box0.has_collision(box1);
+
+}
+
+
+//stackless recursive collision routine
+static void _find_quantized_collision_pairs_recursive(
+ btGImpactQuantizedBvh * boxset0, btGImpactQuantizedBvh * boxset1,
+ btPairSet * collision_pairs,
+ const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+ int node0, int node1, bool complete_primitive_tests)
+{
+
+
+
+ if( _quantized_node_collision(
+ boxset0,boxset1,trans_cache_1to0,
+ node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
+
+ if(boxset0->isLeafNode(node0))
+ {
+ if(boxset1->isLeafNode(node1))
+ {
+ // collision result
+ collision_pairs->push_pair(
+ boxset0->getNodeData(node0),boxset1->getNodeData(node1));
+ return;
+ }
+ else
+ {
+
+ //collide left recursive
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ node0,boxset1->getLeftNode(node1),false);
+
+ //collide right recursive
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ node0,boxset1->getRightNode(node1),false);
+
+
+ }
+ }
+ else
+ {
+ if(boxset1->isLeafNode(node1))
+ {
+
+ //collide left recursive
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getLeftNode(node0),node1,false);
+
+
+ //collide right recursive
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getRightNode(node0),node1,false);
+
+
+ }
+ else
+ {
+ //collide left0 left1
+
+
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
+
+ //collide left0 right1
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
+
+
+ //collide right0 left1
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
+
+ //collide right0 right1
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
+
+ }// else if node1 is not a leaf
+ }// else if node0 is not a leaf
+}
+
+
+void btGImpactQuantizedBvh::find_collision(btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
+ btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
+ btPairSet & collision_pairs)
+{
+
+ if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
+
+ BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
+
+ trans_cache_1to0.calc_from_homogenic(trans0,trans1);
+
+#ifdef TRI_COLLISION_PROFILING
+ bt_begin_gim02_q_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ &collision_pairs,trans_cache_1to0,0,0,true);
+#ifdef TRI_COLLISION_PROFILING
+ bt_end_gim02_q_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGImpactShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,183 @@
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.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 "BulletCollision/Gimpact/btGImpactShape.h"
+#include "BulletCollision/Gimpact/btGImpactMassUtil.h"
+
+
+#define CALC_EXACT_INERTIA 1
+
+void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ lockChildShapes();
+#ifdef CALC_EXACT_INERTIA
+ inertia.setValue(0.f,0.f,0.f);
+
+ int i = this->getNumChildShapes();
+ btScalar shapemass = mass/btScalar(i);
+
+ while(i--)
+ {
+ btVector3 temp_inertia;
+ m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia);
+ if(childrenHasTransform())
+ {
+ inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]);
+ }
+ else
+ {
+ inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity());
+ }
+
+ }
+
+#else
+
+ // Calc box inertia
+
+ btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
+ btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
+ btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
+ const btScalar x2 = lx*lx;
+ const btScalar y2 = ly*ly;
+ const btScalar z2 = lz*lz;
+ const btScalar scaledmass = mass * btScalar(0.08333333);
+
+ inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+#endif
+ unlockChildShapes();
+}
+
+
+
+void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ lockChildShapes();
+
+
+#ifdef CALC_EXACT_INERTIA
+ inertia.setValue(0.f,0.f,0.f);
+
+ int i = this->getVertexCount();
+ btScalar pointmass = mass/btScalar(i);
+
+ while(i--)
+ {
+ btVector3 pointintertia;
+ this->getVertex(i,pointintertia);
+ pointintertia = gim_get_point_inertia(pointintertia,pointmass);
+ inertia+=pointintertia;
+ }
+
+#else
+
+ // Calc box inertia
+
+ btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
+ btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
+ btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
+ const btScalar x2 = lx*lx;
+ const btScalar y2 = ly*ly;
+ const btScalar z2 = lz*lz;
+ const btScalar scaledmass = mass * btScalar(0.08333333);
+
+ inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+#endif
+
+ unlockChildShapes();
+}
+
+void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+
+#ifdef CALC_EXACT_INERTIA
+ inertia.setValue(0.f,0.f,0.f);
+
+ int i = this->getMeshPartCount();
+ btScalar partmass = mass/btScalar(i);
+
+ while(i--)
+ {
+ btVector3 partinertia;
+ getMeshPart(i)->calculateLocalInertia(partmass,partinertia);
+ inertia+=partinertia;
+ }
+
+#else
+
+ // Calc box inertia
+
+ btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
+ btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
+ btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
+ const btScalar x2 = lx*lx;
+ const btScalar y2 = ly*ly;
+ const btScalar z2 = lz*lz;
+ const btScalar scaledmass = mass * btScalar(0.08333333);
+
+ inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+#endif
+}
+
+void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
+{
+}
+
+
+void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+ lockChildShapes();
+ btAABB box;
+ box.m_min = aabbMin;
+ box.m_max = aabbMax;
+
+ btAlignedObjectArray<int> collided;
+ m_box_set.boxQuery(box,collided);
+
+ if(collided.size()==0)
+ {
+ unlockChildShapes();
+ return;
+ }
+
+ int part = (int)getPart();
+ btPrimitiveTriangle triangle;
+ int i = collided.size();
+ while(i--)
+ {
+ this->getPrimitiveTriangle(collided[i],triangle);
+ callback->processTriangle(triangle.m_vertices,part,collided[i]);
+ }
+ unlockChildShapes();
+
+}
+
+void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+ int i = m_mesh_parts.size();
+ while(i--)
+ {
+ m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax);
+ }
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeneric6DofConstraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeneric6DofConstraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeneric6DofConstraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeneric6DofConstraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,893 @@
+/*
+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.
+*/
+/*
+2007-09-09
+Refactored by Francisco Le?n
+email: projectileman at yahoo.com
+http://gimpact.sf.net
+*/
+
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+
+
+#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)
+{
+ int i = index%3;
+ int j = index/3;
+ 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
+ //
+
+ btScalar fi = btGetMatrixElem(mat,2);
+ if (fi < btScalar(1.0f))
+ {
+ if (fi > btScalar(-1.0f))
+ {
+ 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;
+ }
+ }
+ 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)
+ {
+ m_currentLimit = 0;//Free from violation
+ return 0;
+ }
+ if (test_value < m_loLimit)
+ {
+ m_currentLimit = 1;//low limit violation
+ m_currentLimitError = test_value - m_loLimit;
+ return 1;
+ }
+ else if (test_value> m_hiLimit)
+ {
+ m_currentLimit = 2;//High limit violation
+ m_currentLimitError = test_value - m_hiLimit;
+ return 2;
+ };
+
+ m_currentLimit = 0;//Free from violation
+ return 0;
+
+}
+
+
+
+btScalar btRotationalLimitMotor::solveAngularLimits(
+ btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
+ btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
+{
+ if (needApplyTorques()==false) return 0.0f;
+
+ 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;
+ }
+
+ maxMotorForce *= timeStep;
+
+ // 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);
+
+ // correction velocity
+ 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
+ }
+
+
+ // correction impulse
+ btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+
+ // clip correction impulse
+ btScalar clippedMotorImpulse;
+
+ ///@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(-BT_LARGE_FLOAT);
+ btScalar hi = btScalar(BT_LARGE_FLOAT);
+
+ 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;
+
+ //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);
+
+
+ return clippedMotorImpulse;
+
+
+}
+
+//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
+
+
+
+
+//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+
+
+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;
+ }
+
+ 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;
+ };
+
+ m_currentLimit[limitIndex] = 0;//Free from violation
+ m_currentLimitError[limitIndex] = btScalar(0.f);
+ return 0;
+}
+
+
+
+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();
+
+ btVector3 vel1;
+ bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+ btVector3 vel2;
+ bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+ btVector3 vel = vel1 - vel2;
+
+ btScalar rel_vel = axis_normal_on_a.dot(vel);
+
+
+
+ /// apply displacement correction
+
+ //positional error (zeroth order error)
+ btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+ btScalar lo = btScalar(-BT_LARGE_FLOAT);
+ btScalar hi = btScalar(BT_LARGE_FLOAT);
+
+ 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);
+
+ 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.
+ btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
+ btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
+
+ m_calculatedAxis[1] = axis2.cross(axis0);
+ m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+ m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+
+ m_calculatedAxis[0].normalize();
+ m_calculatedAxis[1].normalize();
+ m_calculatedAxis[2].normalize();
+
+}
+
+void btGeneric6DofConstraint::calculateTransforms()
+{
+ calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+ m_calculatedTransformA = transA * m_frameInA;
+ m_calculatedTransformB = transB * m_frameInB;
+ calculateLinearInfo();
+ calculateAngleInfo();
+}
+
+
+
+void btGeneric6DofConstraint::buildLinearJacobian(
+ btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+ const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+{
+ new (&jacLinear) btJacobianEntry(
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ pivotAInW - m_rbA.getCenterOfMassPosition(),
+ pivotBInW - m_rbB.getCenterOfMassPosition(),
+ normalWorld,
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbA.getInvMass(),
+ m_rbB.getInvInertiaDiagLocal(),
+ m_rbB.getInvMass());
+}
+
+
+
+void btGeneric6DofConstraint::buildAngularJacobian(
+ btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+{
+ new (&jacAngular) btJacobianEntry(jointAxisW,
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbB.getInvInertiaDiagLocal());
+
+}
+
+
+
+bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
+{
+ btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+ angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
+ m_angularLimits[axis_index].m_currentPosition = angle;
+ //test limits
+ m_angularLimits[axis_index].testLimitValue(angle);
+ return m_angularLimits[axis_index].needApplyTorques();
+}
+
+
+
+void btGeneric6DofConstraint::buildJacobian()
+{
+#ifndef __SPU__
+ 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(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+
+ // 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);
+
+ }
+ }
+
+ // 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);
+ }
+ }
+
+ }
+#endif //__SPU__
+
+}
+
+
+void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ } else
+ {
+ //prepare constraint
+ calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+ 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::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ } else
+ {
+ //pre-allocate all 6
+ info->m_numConstraintRows = 6;
+ info->nub = 0;
+ }
+}
+
+
+void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
+{
+ getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(),m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
+}
+
+void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+ btAssert(!m_useSolveConstraintObsolete);
+
+ //prepare constraint
+ calculateTransforms(transA,transB);
+
+ int i;
+ //test linear limits
+ for(i = 0; i < 3; i++)
+ {
+ if(m_linearLimits.needApplyForce(i))
+ {
+
+ }
+ }
+ //test angular limits
+ for (i=0;i<3 ;i++ )
+ {
+ if(testAngularLimitMotor(i))
+ {
+
+ }
+ }
+
+ int row = setLinearLimits(info,transA,transB,linVelA,linVelB,angVelA,angVelB);
+ setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+}
+
+
+
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+ 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_currentPosition = m_linearLimits.m_currentLinearDiff[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,
+ transA,transB,linVelA,linVelB,angVelA,angVelB
+ , info, row, axis, 0);
+ }
+ }
+ return row;
+}
+
+
+
+int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+ 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),
+ transA,transB,linVelA,linVelB,angVelA,angVelB,
+ info,row,axis,1);
+ }
+ }
+
+ return row;
+}
+
+
+
+void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+
+
+ m_timeStep = timeStep;
+
+ //calculateTransforms();
+
+ int i;
+
+ // linear
+
+ 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();
+
+ 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;
+
+}
+
+
+
+btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
+{
+ return m_calculatedAxis[axis_index];
+}
+
+
+btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
+{
+ return m_calculatedLinearDiff[axisIndex];
+}
+
+
+btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
+{
+ return m_calculatedAxisAngleDiff[axisIndex];
+}
+
+
+
+void btGeneric6DofConstraint::calcAnchorPos(void)
+{
+ btScalar imA = m_rbA.getInvMass();
+ btScalar imB = m_rbB.getInvMass();
+ btScalar weight;
+ if(imB == btScalar(0.0))
+ {
+ weight = btScalar(1.0);
+ }
+ else
+ {
+ weight = imA / (imA + imB);
+ }
+ const btVector3& pA = m_calculatedTransformA.getOrigin();
+ const btVector3& pB = m_calculatedTransformB.getOrigin();
+ m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
+ return;
+}
+
+
+
+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.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
+ m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
+ }
+}
+
+
+
+int btGeneric6DofConstraint::get_limit_motor_info2(
+ btRotationalLimitMotor * limot,
+ const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+ 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))
+ {
+ btVector3 ltd; // Linear Torque Decoupling vector
+ btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
+ 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() - transB.getOrigin();
+ 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)
+ {
+ btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
+
+ btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
+ limot->m_loLimit,
+ limot->m_hiLimit,
+ tag_vel,
+ info->fps * info->erp);
+ info->m_constraintError[srow] += mot_fact * 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 = angVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+// if (body1)
+ vel -= angVelB.dot(ax1);
+ }
+ else
+ {
+ vel = linVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+// if (body1)
+ vel -= linVelB.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;
+}
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeneric6DofSpringConstraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeneric6DofSpringConstraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeneric6DofSpringConstraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeneric6DofSpringConstraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,144 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+
+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 "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
+ : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
+{
+ for(int i = 0; i < 6; i++)
+ {
+ m_springEnabled[i] = false;
+ m_equilibriumPoint[i] = btScalar(0.f);
+ m_springStiffness[i] = btScalar(0.f);
+ m_springDamping[i] = btScalar(1.f);
+ }
+}
+
+
+void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
+{
+ btAssert((index >= 0) && (index < 6));
+ m_springEnabled[index] = onOff;
+ if(index < 3)
+ {
+ m_linearLimits.m_enableMotor[index] = onOff;
+ }
+ else
+ {
+ m_angularLimits[index - 3].m_enableMotor = onOff;
+ }
+}
+
+
+
+void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
+{
+ btAssert((index >= 0) && (index < 6));
+ m_springStiffness[index] = stiffness;
+}
+
+
+void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
+{
+ btAssert((index >= 0) && (index < 6));
+ m_springDamping[index] = damping;
+}
+
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint()
+{
+ calculateTransforms();
+ for(int i = 0; i < 3; i++)
+ {
+ m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
+ }
+ for(int i = 0; i < 3; i++)
+ {
+ m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
+ }
+}
+
+
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
+{
+ btAssert((index >= 0) && (index < 6));
+ calculateTransforms();
+ if(index < 3)
+ {
+ m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
+ }
+ else
+ {
+ m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index];
+ }
+}
+
+
+
+void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
+{
+ // it is assumed that calculateTransforms() have been called before this call
+ int i;
+ btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
+ for(i = 0; i < 3; i++)
+ {
+ if(m_springEnabled[i])
+ {
+ // get current position of constraint
+ btScalar currPos = m_calculatedLinearDiff[i];
+ // calculate difference
+ btScalar delta = currPos - m_equilibriumPoint[i];
+ // spring force is (delta * m_stiffness) according to Hooke's Law
+ btScalar force = delta * m_springStiffness[i];
+ btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
+ m_linearLimits.m_targetVelocity[i] = velFactor * force;
+ m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps;
+ }
+ }
+ for(i = 0; i < 3; i++)
+ {
+ if(m_springEnabled[i + 3])
+ {
+ // get current position of constraint
+ btScalar currPos = m_calculatedAxisAngleDiff[i];
+ // calculate difference
+ btScalar delta = currPos - m_equilibriumPoint[i+3];
+ // spring force is (-delta * m_stiffness) according to Hooke's Law
+ btScalar force = -delta * m_springStiffness[i+3];
+ btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
+ m_angularLimits[i].m_targetVelocity = velFactor * force;
+ m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
+ }
+ }
+}
+
+
+void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
+{
+ // this will be called by constraint solver at the constraint setup stage
+ // set current motor parameters
+ internalUpdateSprings(info);
+ // do the rest of job for constraint setup
+ btGeneric6DofConstraint::getInfo2(info);
+}
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGenericPoolAllocator.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGenericPoolAllocator.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGenericPoolAllocator.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGenericPoolAllocator.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,283 @@
+/*! \file btGenericPoolAllocator.cpp
+\author Francisco Len Nßjera. email projectileman at yahoo.com
+
+General purpose allocator class
+*/
+/*
+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 "BulletCollision/Gimpact/btGenericPoolAllocator.h"
+
+
+
+/// *************** btGenericMemoryPool ******************///////////
+
+size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements)
+{
+ size_t ptr = BT_UINT_MAX;
+
+ if(m_free_nodes_count == 0) return BT_UINT_MAX;
+ // find an avaliable free node with the correct size
+ size_t revindex = m_free_nodes_count;
+
+ while(revindex-- && ptr == BT_UINT_MAX)
+ {
+ if(m_allocated_sizes[m_free_nodes[revindex]]>=num_elements)
+ {
+ ptr = revindex;
+ }
+ }
+ if(ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found
+
+
+ revindex = ptr;
+ ptr = m_free_nodes[revindex];
+ // post: ptr contains the node index, and revindex the index in m_free_nodes
+
+ size_t finalsize = m_allocated_sizes[ptr];
+ finalsize -= num_elements;
+
+ m_allocated_sizes[ptr] = num_elements;
+
+ // post: finalsize>=0, m_allocated_sizes[ptr] has the requested size
+
+ if(finalsize>0) // preserve free node, there are some free memory
+ {
+ m_free_nodes[revindex] = ptr + num_elements;
+ m_allocated_sizes[ptr + num_elements] = finalsize;
+ }
+ else // delete free node
+ {
+ // swap with end
+ m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count-1];
+ m_free_nodes_count--;
+ }
+
+ return ptr;
+}
+
+size_t btGenericMemoryPool::allocate_from_pool(size_t num_elements)
+{
+ if(m_allocated_count+num_elements>m_max_element_count) return BT_UINT_MAX;
+
+ size_t ptr = m_allocated_count;
+
+ m_allocated_sizes[m_allocated_count] = num_elements;
+ m_allocated_count+=num_elements;
+
+ return ptr;
+}
+
+
+void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count)
+{
+ m_allocated_count = 0;
+ m_free_nodes_count = 0;
+
+ m_element_size = element_size;
+ m_max_element_count = element_count;
+
+
+
+
+ m_pool = (unsigned char *) btAlignedAlloc(m_element_size*m_max_element_count,16);
+ m_free_nodes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16);
+ m_allocated_sizes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16);
+
+ for (size_t i = 0;i< m_max_element_count;i++ )
+ {
+ m_allocated_sizes[i] = 0;
+ }
+}
+
+void btGenericMemoryPool::end_pool()
+{
+ btAlignedFree(m_pool);
+ btAlignedFree(m_free_nodes);
+ btAlignedFree(m_allocated_sizes);
+ m_allocated_count = 0;
+ m_free_nodes_count = 0;
+}
+
+
+//! Allocates memory in pool
+/*!
+\param size_bytes size in bytes of the buffer
+*/
+void * btGenericMemoryPool::allocate(size_t size_bytes)
+{
+
+ size_t module = size_bytes%m_element_size;
+ size_t element_count = size_bytes/m_element_size;
+ if(module>0) element_count++;
+
+ size_t alloc_pos = allocate_from_free_nodes(element_count);
+ // a free node is found
+ if(alloc_pos != BT_UINT_MAX)
+ {
+ return get_element_data(alloc_pos);
+ }
+ // allocate directly on pool
+ alloc_pos = allocate_from_pool(element_count);
+
+ if(alloc_pos == BT_UINT_MAX) return NULL; // not space
+ return get_element_data(alloc_pos);
+}
+
+bool btGenericMemoryPool::freeMemory(void * pointer)
+{
+ unsigned char * pointer_pos = (unsigned char *)pointer;
+ unsigned char * pool_pos = (unsigned char *)m_pool;
+ // calc offset
+ if(pointer_pos<pool_pos) return false;//other pool
+ size_t offset = size_t(pointer_pos - pool_pos);
+ if(offset>=get_pool_capacity()) return false;// far away
+
+ // find free position
+ m_free_nodes[m_free_nodes_count] = offset/m_element_size;
+ m_free_nodes_count++;
+ return true;
+}
+
+
+/// *******************! btGenericPoolAllocator *******************!///
+
+
+btGenericPoolAllocator::~btGenericPoolAllocator()
+{
+ // destroy pools
+ size_t i;
+ for (i=0;i<m_pool_count;i++)
+ {
+ m_pools[i]->end_pool();
+ btAlignedFree(m_pools[i]);
+ }
+}
+
+
+// creates a pool
+btGenericMemoryPool * btGenericPoolAllocator::push_new_pool()
+{
+ if(m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL;
+
+ btGenericMemoryPool * newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool),16);
+
+ m_pools[m_pool_count] = newptr;
+
+ m_pools[m_pool_count]->init_pool(m_pool_element_size,m_pool_element_count);
+
+ m_pool_count++;
+ return newptr;
+}
+
+void * btGenericPoolAllocator::failback_alloc(size_t size_bytes)
+{
+
+ btGenericMemoryPool * pool = NULL;
+
+
+ if(size_bytes<=get_pool_capacity())
+ {
+ pool = push_new_pool();
+ }
+
+ if(pool==NULL) // failback
+ {
+ return btAlignedAlloc(size_bytes,16);
+ }
+
+ return pool->allocate(size_bytes);
+}
+
+bool btGenericPoolAllocator::failback_free(void * pointer)
+{
+ btAlignedFree(pointer);
+ return true;
+}
+
+
+//! Allocates memory in pool
+/*!
+\param size_bytes size in bytes of the buffer
+*/
+void * btGenericPoolAllocator::allocate(size_t size_bytes)
+{
+ void * ptr = NULL;
+
+ size_t i = 0;
+ while(i<m_pool_count && ptr == NULL)
+ {
+ ptr = m_pools[i]->allocate(size_bytes);
+ ++i;
+ }
+
+ if(ptr) return ptr;
+
+ return failback_alloc(size_bytes);
+}
+
+bool btGenericPoolAllocator::freeMemory(void * pointer)
+{
+ bool result = false;
+
+ size_t i = 0;
+ while(i<m_pool_count && result == false)
+ {
+ result = m_pools[i]->freeMemory(pointer);
+ ++i;
+ }
+
+ if(result) return true;
+
+ return failback_free(pointer);
+}
+
+/// ************** STANDARD ALLOCATOR ***************************///
+
+
+#define BT_DEFAULT_POOL_SIZE 32768
+#define BT_DEFAULT_POOL_ELEMENT_SIZE 8
+
+// main allocator
+class GIM_STANDARD_ALLOCATOR: public btGenericPoolAllocator
+{
+public:
+ GIM_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE)
+ {
+ }
+};
+
+// global allocator
+GIM_STANDARD_ALLOCATOR g_main_allocator;
+
+
+void * btPoolAlloc(size_t size)
+{
+ return g_main_allocator.allocate(size);
+}
+
+void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize)
+{
+ void * newptr = btPoolAlloc(newsize);
+ size_t copysize = oldsize<newsize?oldsize:newsize;
+ memcpy(newptr,ptr,copysize);
+ btPoolFree(ptr);
+ return newptr;
+}
+
+void btPoolFree(void *ptr)
+{
+ g_main_allocator.freeMemory(ptr);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeometryUtil.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeometryUtil.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeometryUtil.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGeometryUtil.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,185 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "LinearMath/btGeometryUtil.h"
+
+
+/*
+ Make sure this dummy function never changes so that it
+ can be used by probes that are checking whether the
+ library is actually installed.
+*/
+extern "C"
+{
+ void btBulletMathProbe ();
+
+ void btBulletMathProbe () {}
+}
+
+
+bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin)
+{
+ int numbrushes = planeEquations.size();
+ for (int i=0;i<numbrushes;i++)
+ {
+ const btVector3& N1 = planeEquations[i];
+ btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin;
+ if (dist>btScalar(0.))
+ {
+ return false;
+ }
+ }
+ return true;
+
+}
+
+
+bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin)
+{
+ int numvertices = vertices.size();
+ for (int i=0;i<numvertices;i++)
+ {
+ const btVector3& N1 = vertices[i];
+ btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
+ if (dist>btScalar(0.))
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations);
+
+bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
+{
+ int numbrushes = planeEquations.size();
+ for (int i=0;i<numbrushes;i++)
+ {
+ const btVector3& N1 = planeEquations[i];
+ if (planeEquation.dot(N1) > btScalar(0.999))
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
+void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
+{
+ const int numvertices = vertices.size();
+ // brute force:
+ for (int i=0;i<numvertices;i++)
+ {
+ const btVector3& N1 = vertices[i];
+
+
+ for (int j=i+1;j<numvertices;j++)
+ {
+ const btVector3& N2 = vertices[j];
+
+ for (int k=j+1;k<numvertices;k++)
+ {
+
+ const btVector3& N3 = vertices[k];
+
+ btVector3 planeEquation,edge0,edge1;
+ edge0 = N2-N1;
+ edge1 = N3-N1;
+ btScalar normalSign = btScalar(1.);
+ for (int ww=0;ww<2;ww++)
+ {
+ planeEquation = normalSign * edge0.cross(edge1);
+ if (planeEquation.length2() > btScalar(0.0001))
+ {
+ planeEquation.normalize();
+ if (notExist(planeEquation,planeEquationsOut))
+ {
+ planeEquation[3] = -planeEquation.dot(N1);
+
+ //check if inside, and replace supportingVertexOut if needed
+ if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
+ {
+ planeEquationsOut.push_back(planeEquation);
+ }
+ }
+ }
+ normalSign = btScalar(-1.);
+ }
+
+ }
+ }
+ }
+
+}
+
+void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
+{
+ const int numbrushes = planeEquations.size();
+ // brute force:
+ for (int i=0;i<numbrushes;i++)
+ {
+ const btVector3& N1 = planeEquations[i];
+
+
+ for (int j=i+1;j<numbrushes;j++)
+ {
+ const btVector3& N2 = planeEquations[j];
+
+ for (int k=j+1;k<numbrushes;k++)
+ {
+
+ const btVector3& N3 = planeEquations[k];
+
+ btVector3 n2n3; n2n3 = N2.cross(N3);
+ btVector3 n3n1; n3n1 = N3.cross(N1);
+ btVector3 n1n2; n1n2 = N1.cross(N2);
+
+ if ( ( n2n3.length2() > btScalar(0.0001) ) &&
+ ( n3n1.length2() > btScalar(0.0001) ) &&
+ ( n1n2.length2() > btScalar(0.0001) ) )
+ {
+ //point P out of 3 plane equations:
+
+ // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
+ //P = -------------------------------------------------------------------------
+ // N1 . ( N2 * N3 )
+
+
+ btScalar quotient = (N1.dot(n2n3));
+ if (btFabs(quotient) > btScalar(0.000001))
+ {
+ quotient = btScalar(-1.) / quotient;
+ n2n3 *= N1[3];
+ n3n1 *= N2[3];
+ n1n2 *= N3[3];
+ btVector3 potentialVertex = n2n3;
+ potentialVertex += n3n1;
+ potentialVertex += n1n2;
+ potentialVertex *= quotient;
+
+ //check if inside, and replace supportingVertexOut if needed
+ if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
+ {
+ verticesOut.push_back(potentialVertex);
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGhostObject.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGhostObject.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGhostObject.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGhostObject.cpp Sat Dec 19 14:05:59 2009
@@ -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 "BulletCollision/CollisionDispatch/btGhostObject.h"
+#include "BulletCollision/CollisionDispatch/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<m_overlappingObjects.size())
+ {
+ m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
+ m_overlappingObjects.pop_back();
+ }
+}
+
+
+btPairCachingGhostObject::btPairCachingGhostObject()
+{
+ m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+}
+
+btPairCachingGhostObject::~btPairCachingGhostObject()
+{
+ m_hashPairCache->~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 (index<m_overlappingObjects.size())
+ {
+ m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
+ m_overlappingObjects.pop_back();
+ m_hashPairCache->removeOverlappingPair(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;i<m_overlappingObjects.size();i++)
+ {
+ btCollisionObject* collisionObject= m_overlappingObjects[i];
+ //only perform raycast if filterMask matches
+ if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
+ //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;i<m_overlappingObjects.size();i++)
+ {
+ btCollisionObject* collisionObject= m_overlappingObjects[i];
+ //only perform raycast if filterMask matches
+ if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
+ {
+ btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,
+ collisionObject,
+ collisionObject->getCollisionShape(),
+ collisionObject->getWorldTransform(),
+ resultCallback);
+ }
+ }
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkConvexCast.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkConvexCast.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkConvexCast.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkConvexCast.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,176 @@
+/*
+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 "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
+#include "LinearMath/btTransformUtil.h"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#else
+#define MAX_ITERATIONS 32
+#endif
+
+btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),
+m_convexB(convexB)
+{
+}
+
+bool btGjkConvexCast::calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result)
+{
+
+
+ m_simplexSolver->reset();
+
+ /// compute linear velocity for this interval, to interpolate
+ //assume no rotation/angular velocity, assert here?
+ btVector3 linVelA,linVelB;
+ linVelA = toA.getOrigin()-fromA.getOrigin();
+ linVelB = toB.getOrigin()-fromB.getOrigin();
+
+ btScalar radius = btScalar(0.001);
+ btScalar lambda = btScalar(0.);
+ btVector3 v(1,0,0);
+
+ int maxIter = MAX_ITERATIONS;
+
+ btVector3 n;
+ n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ bool hasResult = false;
+ btVector3 c;
+ btVector3 r = (linVelA-linVelB);
+
+ btScalar lastLambda = lambda;
+ //btScalar epsilon = btScalar(0.001);
+
+ int numIter = 0;
+ //first solution, using GJK
+
+
+ btTransform identityTrans;
+ identityTrans.setIdentity();
+
+
+// result.drawCoordSystem(sphereTr);
+
+ btPointCollector pointCollector;
+
+
+ btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);
+ btGjkPairDetector::ClosestPointInput input;
+
+ //we don't use margins during CCD
+ // gjk.setIgnoreMargin(true);
+
+ input.m_transformA = fromA;
+ input.m_transformB = fromB;
+ gjk.getClosestPoints(input,pointCollector,0);
+
+ hasResult = pointCollector.m_hasResult;
+ c = pointCollector.m_pointInWorld;
+
+ if (hasResult)
+ {
+ btScalar dist;
+ dist = pointCollector.m_distance;
+ n = pointCollector.m_normalOnBInWorld;
+
+
+
+ //not close enough
+ while (dist > radius)
+ {
+ numIter++;
+ if (numIter > maxIter)
+ {
+ return false; //todo: report a failure
+ }
+ btScalar dLambda = btScalar(0.);
+
+ btScalar projectedLinearVelocity = r.dot(n);
+
+ dLambda = dist / (projectedLinearVelocity);
+
+ lambda = lambda - dLambda;
+
+ if (lambda > btScalar(1.))
+ return false;
+
+ if (lambda < btScalar(0.))
+ return false;
+
+ //todo: next check with relative epsilon
+ if (lambda <= lastLambda)
+ {
+ return false;
+ //n.setValue(0,0,0);
+ break;
+ }
+ lastLambda = lambda;
+
+ //interpolate to next lambda
+ result.DebugDraw( lambda );
+ input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
+ input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
+
+ gjk.getClosestPoints(input,pointCollector,0);
+ if (pointCollector.m_hasResult)
+ {
+ if (pointCollector.m_distance < btScalar(0.))
+ {
+ result.m_fraction = lastLambda;
+ n = pointCollector.m_normalOnBInWorld;
+ result.m_normal=n;
+ result.m_hitPoint = pointCollector.m_pointInWorld;
+ return true;
+ }
+ c = pointCollector.m_pointInWorld;
+ n = pointCollector.m_normalOnBInWorld;
+ dist = pointCollector.m_distance;
+ } else
+ {
+ //??
+ return false;
+ }
+
+ }
+
+ //is n normalized?
+ //don't report time of impact for motion away from the contact normal (or causes minor penetration)
+ if (n.dot(r)>=-result.m_allowedPenetration)
+ return false;
+
+ result.m_fraction = lambda;
+ result.m_normal = n;
+ result.m_hitPoint = c;
+ return true;
+ }
+
+ return false;
+
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkEpa2.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkEpa2.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkEpa2.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkEpa2.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,989 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 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.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson, 2008
+*/
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#endif //__SPU__
+#endif
+
+namespace gjkepa2_impl
+{
+
+ // Config
+
+ /* GJK */
+#define GJK_MAX_ITERATIONS 128
+#define GJK_ACCURARY ((btScalar)0.0001)
+#define GJK_MIN_DISTANCE ((btScalar)0.0001)
+#define GJK_DUPLICATED_EPS ((btScalar)0.0001)
+#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
+#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
+#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
+
+ /* EPA */
+#define EPA_MAX_VERTICES 64
+#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
+#define EPA_MAX_ITERATIONS 255
+#define EPA_ACCURACY ((btScalar)0.0001)
+#define EPA_FALLBACK (10*EPA_ACCURACY)
+#define EPA_PLANE_EPS ((btScalar)0.00001)
+#define EPA_INSIDE_EPS ((btScalar)0.01)
+
+
+ // Shorthands
+ typedef unsigned int U;
+ typedef unsigned char U1;
+
+ // MinkowskiDiff
+ struct MinkowskiDiff
+ {
+ const btConvexShape* m_shapes[2];
+ btMatrix3x3 m_toshape1;
+ btTransform m_toshape0;
+#ifdef __SPU__
+ bool m_enableMargin;
+#else
+ btVector3 (btConvexShape::*Ls)(const btVector3&) const;
+#endif//__SPU__
+
+
+ MinkowskiDiff()
+ {
+
+ }
+#ifdef __SPU__
+ void EnableMargin(bool enable)
+ {
+ m_enableMargin = enable;
+ }
+ inline btVector3 Support0(const btVector3& d) const
+ {
+ if (m_enableMargin)
+ {
+ return m_shapes[0]->localGetSupportVertexNonVirtual(d);
+ } else
+ {
+ return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
+ }
+ }
+ inline btVector3 Support1(const btVector3& d) const
+ {
+ if (m_enableMargin)
+ {
+ return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
+ } else
+ {
+ return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
+ }
+ }
+#else
+ void EnableMargin(bool enable)
+ {
+ if(enable)
+ Ls=&btConvexShape::localGetSupportVertexNonVirtual;
+ else
+ Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
+ }
+ inline btVector3 Support0(const btVector3& d) const
+ {
+ return(((m_shapes[0])->*(Ls))(d));
+ }
+ inline btVector3 Support1(const btVector3& d) const
+ {
+ return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
+ }
+#endif //__SPU__
+
+ inline btVector3 Support(const btVector3& d) const
+ {
+ return(Support0(d)-Support1(-d));
+ }
+ btVector3 Support(const btVector3& d,U index) const
+ {
+ if(index)
+ return(Support1(d));
+ else
+ return(Support0(d));
+ }
+ };
+
+ 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()
+ {
+ 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(rl<GJK_MIN_DISTANCE)
+ {/* Touching or inside */
+ m_status=eStatus::Inside;
+ break;
+ }
+ /* Append new vertice in -'v' direction */
+ appendvertice(cs,-m_ray);
+ const btVector3& w=cs.c[cs.rank-1]->w;
+ bool found=false;
+ for(U i=0;i<4;++i)
+ {
+ if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
+ { found=true;break; }
+ }
+ if(found)
+ {/* Return old simplex */
+ removevertice(m_simplices[m_current]);
+ break;
+ }
+ else
+ {/* Update lastw */
+ lastw[clastw=(clastw+1)&3]=w;
+ }
+ /* Check for termination */
+ const btScalar omega=btDot(m_ray,w)/rl;
+ alpha=btMax(omega,alpha);
+ if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+ {/* Return old simplex */
+ removevertice(m_simplices[m_current]);
+ break;
+ }
+ /* Reduce simplex */
+ btScalar weights[4];
+ U mask=0;
+ switch(cs.rank)
+ {
+ case 2: sqdist=projectorigin( cs.c[0]->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(sqdist>=0)
+ {/* Valid */
+ ns.rank = 0;
+ m_ray = btVector3(0,0,0);
+ m_current = next;
+ for(U i=0,ni=cs.rank;i<ni;++i)
+ {
+ if(mask&(1<<i))
+ {
+ ns.c[ns.rank] = cs.c[i];
+ ns.p[ns.rank++] = weights[i];
+ m_ray += cs.c[i]->w*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)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
+ } while(m_status==eStatus::Valid);
+ m_simplex=&m_simplices[m_current];
+ switch(m_status)
+ {
+ case eStatus::Valid: m_distance=m_ray.length();break;
+ case eStatus::Inside: m_distance=0;break;
+ default:
+ {
+ }
+ }
+ return(m_status);
+ }
+ bool EncloseOrigin()
+ {
+ switch(m_simplex->rank)
+ {
+ 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=btCross(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=btCross(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:
+ {
+ 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);
+ }
+ /* 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?-btDot(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=btCross(dl[0],dl[1]);
+ const btScalar l=n.length2();
+ if(l>GJK_SIMPLEX3_EPS)
+ {
+ btScalar mindist=-1;
+ btScalar subw[2]={0.f,0.f};
+ U subm(0);
+ for(U i=0;i<3;++i)
+ {
+ if(btDot(*vt[i],btCross(dl[i],n))>0)
+ {
+ const U j=imd3[i];
+ const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
+ if((mindist<0)||(subd<mindist))
+ {
+ mindist = subd;
+ m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
+ w[i] = subw[0];
+ w[j] = subw[1];
+ w[imd3[j]] = 0;
+ }
+ }
+ }
+ if(mindist<0)
+ {
+ const btScalar d=btDot(a,n);
+ const btScalar s=btSqrt(l);
+ const btVector3 p=n*(d/l);
+ mindist = p.length2();
+ m = 7;
+ w[0] = (btCross(dl[1],b-p)).length()/s;
+ w[1] = (btCross(dl[2],c-p)).length()/s;
+ w[2] = 1-(w[0]+w[1]);
+ }
+ return(mindist);
+ }
+ return(-1);
+ }
+ static btScalar projectorigin( const btVector3& a,
+ const btVector3& b,
+ const btVector3& c,
+ const btVector3& d,
+ btScalar* w,U& m)
+ {
+ 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*btDot(a,btCross(b-c,a-b)))<=0;
+ if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
+ {
+ btScalar mindist=-1;
+ btScalar subw[3]={0.f,0.f,0.f};
+ U subm(0);
+ for(U i=0;i<3;++i)
+ {
+ const U j=imd3[i];
+ const btScalar s=vl*btDot(d,btCross(dl[i],dl[j]));
+ if(s>0)
+ {
+ const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
+ if((mindist<0)||(subd<mindist))
+ {
+ mindist = subd;
+ m = static_cast<U>((subm&1?1<<i:0)+
+ (subm&2?1<<j:0)+
+ (subm&4?8:0));
+ w[i] = subw[0];
+ w[j] = subw[1];
+ w[imd3[j]] = 0;
+ w[3] = subw[2];
+ }
+ }
+ }
+ if(mindist<0)
+ {
+ mindist = 0;
+ m = 15;
+ w[0] = det(c,b,d)/vl;
+ w[1] = det(a,c,d)/vl;
+ w[2] = det(b,a,d)/vl;
+ w[3] = 1-(w[0]+w[1]+w[2]);
+ }
+ return(mindist);
+ }
+ return(-1);
+ }
+ };
+
+ // EPA
+ struct EPA
+ {
+ /* Types */
+ typedef GJK::sSV sSV;
+ struct sFace
+ {
+ 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();
+ }
+
+
+ static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
+ {
+ fa->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)
+ {
+ 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;i<EPA_MAX_FACES;++i)
+ {
+ append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
+ }
+ }
+ eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
+ {
+ GJK::sSimplex& simplex=*gjk.m_simplex;
+ if((simplex.rank>1)&&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(;iterations<EPA_MAX_ITERATIONS;++iterations)
+ {
+ if(m_nextsv<EPA_MAX_VERTICES)
+ {
+ sHorizon horizon;
+ sSV* w=&m_sv_store[m_nextsv++];
+ bool valid=true;
+ best->pass = (U1)(++pass);
+ gjk.getsupport(best->n,*w);
+ const btScalar wdist=btDot(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; }
+ }
+ 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] = btCross( outer.c[1]->w-projection,
+ outer.c[2]->w-projection).length();
+ m_result.p[1] = btCross( outer.c[2]->w-projection,
+ outer.c[0]->w-projection).length();
+ m_result.p[2] = btCross( 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);
+ }
+ 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 = btCross(b->w-a->w,c->w-a->w);
+ const btScalar l=face->n.length();
+ const bool v=l>EPA_ACCURACY;
+ face->p = btMin(btMin(
+ btDot(a->w,btCross(face->n,a->w-b->w)),
+ btDot(b->w,btCross(face->n,b->w-c->w))),
+ btDot(c->w,btCross(face->n,c->w-a->w))) /
+ (v?l:1);
+ face->p = face->p>=-EPA_INSIDE_EPS?0:face->p;
+ if(v)
+ {
+ face->d = btDot(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);
+ }
+ 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)&&(sqd<mind))
+ {
+ minf=f;
+ mind=sqd;
+ maxp=f->p;
+ }
+ }
+ 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((btDot(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);
+ }
+
+ };
+
+ //
+ 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);
+ }
+
+}
+
+//
+// Api
+//
+
+using namespace gjkepa2_impl;
+
+//
+int btGjkEpaSolver2::StackSizeRequirement()
+{
+ 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)
+{
+ 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;i<gjk.m_simplex->rank;++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;
+ }
+ 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);
+ }
+}
+
+//
+bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0,
+ 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)
+ {
+ case GJK::eStatus::Inside:
+ {
+ 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;i<epa.m_result.rank;++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);
+ } else results.status=sResults::EPA_Failed;
+ }
+ break;
+ case GJK::eStatus::Failed:
+ results.status=sResults::GJK_Failed;
+ break;
+ default:
+ {
+ }
+ }
+ return(false);
+}
+
+#ifndef __SPU__
+//
+btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
+ btScalar margin,
+ const btConvexShape* shape0,
+ 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)
+ {
+ btVector3 w0=btVector3(0,0,0);
+ btVector3 w1=btVector3(0,0,0);
+ for(U i=0;i<gjk.m_simplex->rank;++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;
+ }
+ 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(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);
+ }
+ }
+ }
+ return(SIMD_INFINITY);
+}
+
+//
+bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
+ const btTransform& wtrs0,
+ const btConvexShape* shape1,
+ const btTransform& wtrs1,
+ const btVector3& guess,
+ sResults& results)
+{
+ if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
+ return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
+ else
+ return(true);
+}
+#endif //__SPU__
+
+/* Symbols cleanup */
+
+#undef GJK_MAX_ITERATIONS
+#undef GJK_ACCURARY
+#undef GJK_MIN_DISTANCE
+#undef GJK_DUPLICATED_EPS
+#undef GJK_SIMPLEX2_EPS
+#undef GJK_SIMPLEX3_EPS
+#undef GJK_SIMPLEX4_EPS
+
+#undef EPA_MAX_VERTICES
+#undef EPA_MAX_FACES
+#undef EPA_MAX_ITERATIONS
+#undef EPA_ACCURACY
+#undef EPA_FALLBACK
+#undef EPA_PLANE_EPS
+#undef EPA_INSIDE_EPS
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkEpaPenetrationDepthSolver.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkEpaPenetrationDepthSolver.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkEpaPenetrationDepthSolver.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkEpaPenetrationDepthSolver.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006
+
+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 "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.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, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
+ class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc )
+{
+
+ (void)debugDraw;
+ (void)v;
+ (void)simplexSolver;
+
+// const btScalar radialmargin(btScalar(0.));
+
+ btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin());
+ btGjkEpaSolver2::sResults results;
+
+
+ if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
+ pConvexB,transformB,
+ guessVector,results))
+
+ {
+ // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+ //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+ wWitnessOnA = results.witnesses[0];
+ wWitnessOnB = results.witnesses[1];
+ v = results.normal;
+ return true;
+ } else
+ {
+ if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results))
+ {
+ wWitnessOnA = results.witnesses[0];
+ wWitnessOnB = results.witnesses[1];
+ v = results.normal;
+ return false;
+ }
+ }
+
+ return false;
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkPairDetector.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkPairDetector.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkPairDetector.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btGjkPairDetector.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,456 @@
+/*
+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 "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+
+
+
+#if defined(DEBUG) || defined (_DEBUG)
+//#define TEST_NON_VIRTUAL 1
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+//#define DEBUG_SPU_COLLISION_DETECTION 1
+#endif //__SPU__
+#endif
+
+//must be above the machine epsilon
+#define REL_ERROR2 btScalar(1.0e-6)
+
+//temp globals, to improve GJK/EPA/penetration calculations
+int gNumDeepPenetrationChecks = 0;
+int gNumGjkChecks = 0;
+
+
+btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
+:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_shapeTypeA(objectA->getShapeType()),
+m_shapeTypeB(objectB->getShapeType()),
+m_marginA(objectA->getMargin()),
+m_marginB(objectB->getMargin()),
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(1)
+{
+}
+btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
+:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_shapeTypeA(shapeTypeA),
+m_shapeTypeB(shapeTypeB),
+m_marginA(marginA),
+m_marginB(marginB),
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(1)
+{
+}
+
+void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+{
+ (void)swapResults;
+
+ getClosestPointsNonVirtual(input,output,debugDraw);
+}
+
+#ifdef __SPU__
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+#else
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+#endif
+{
+ m_cachedSeparatingDistance = 0.f;
+
+ btScalar distance=btScalar(0.);
+ btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
+ btVector3 pointOnA,pointOnB;
+ btTransform localTransA = input.m_transformA;
+ btTransform localTransB = input.m_transformB;
+ btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
+ localTransA.getOrigin() -= positionOffset;
+ localTransB.getOrigin() -= positionOffset;
+
+ bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
+
+ btScalar marginA = m_marginA;
+ btScalar marginB = m_marginB;
+
+ 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;
+ int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
+ m_cachedSeparatingAxis.setValue(0,1,0);
+
+ bool isValid = false;
+ bool checkSimplex = false;
+ bool checkPenetration = true;
+ m_degenerateSimplex = 0;
+
+ m_lastUsedMethod = -1;
+
+ {
+ btScalar squaredDistance = BT_LARGE_FLOAT;
+ btScalar delta = btScalar(0.);
+
+ btScalar margin = marginA + marginB;
+
+
+
+ m_simplexSolver->reset();
+
+ for ( ; ; )
+ //while (true)
+ {
+
+ btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
+ btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+
+#if 1
+
+ btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+ btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+// btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
+// btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
+
+#else
+#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);
+#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__
+#endif
+
+
+ btVector3 pWorld = localTransA(pInA);
+ btVector3 qWorld = localTransB(qInB);
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+ spu_printf("got local supporting vertices\n");
+#endif
+
+ if (check2d)
+ {
+ pWorld[2] = 0.f;
+ qWorld[2] = 0.f;
+ }
+
+ 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))
+ {
+ m_degenerateSimplex = 10;
+ checkSimplex=true;
+ //checkPenetration = false;
+ break;
+ }
+
+ //exit 0: the new point is already in the simplex, or we didn't come any closer
+ if (m_simplexSolver->inSimplex(w))
+ {
+ m_degenerateSimplex = 1;
+ checkSimplex = true;
+ break;
+ }
+ // are we getting any closer ?
+ btScalar f0 = squaredDistance - delta;
+ btScalar f1 = squaredDistance * REL_ERROR2;
+
+ if (f0 <= f1)
+ {
+ if (f0 <= btScalar(0.))
+ {
+ m_degenerateSimplex = 2;
+ } else
+ {
+ m_degenerateSimplex = 11;
+ }
+ 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
+ btVector3 newCachedSeparatingAxis;
+
+ //calculate the closest point to the origin (update vector v)
+ if (!m_simplexSolver->closest(newCachedSeparatingAxis))
+ {
+ m_degenerateSimplex = 3;
+ checkSimplex = true;
+ break;
+ }
+
+ if(newCachedSeparatingAxis.length2()<REL_ERROR2)
+ {
+ m_cachedSeparatingAxis = newCachedSeparatingAxis;
+ m_degenerateSimplex = 6;
+ checkSimplex = true;
+ break;
+ }
+
+ btScalar previousSquaredDistance = squaredDistance;
+ squaredDistance = newCachedSeparatingAxis.length2();
+#if 0
+///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
+ if (squaredDistance>previousSquaredDistance)
+ {
+ m_degenerateSimplex = 7;
+ squaredDistance = previousSquaredDistance;
+ checkSimplex = false;
+ break;
+ }
+#endif //
+
+ m_cachedSeparatingAxis = newCachedSeparatingAxis;
+
+ //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+
+ //are we getting any closer ?
+ if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
+ {
+ m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+ checkSimplex = true;
+ m_degenerateSimplex = 12;
+
+ break;
+ }
+
+ //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
+ if (m_curIter++ > gGjkMaxIter)
+ {
+ #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",
+ m_cachedSeparatingAxis.getX(),
+ m_cachedSeparatingAxis.getY(),
+ m_cachedSeparatingAxis.getZ(),
+ squaredDistance,
+ m_minkowskiA->getShapeType(),
+ m_minkowskiB->getShapeType());
+
+ #endif
+ break;
+
+ }
+
+
+ bool check = (!m_simplexSolver->fullSimplex());
+ //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+
+ if (!check)
+ {
+ //do we need this backup_closest here ?
+ m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+ m_degenerateSimplex = 13;
+ break;
+ }
+ }
+
+ if (checkSimplex)
+ {
+ m_simplexSolver->compute_points(pointOnA, pointOnB);
+ normalInB = pointOnA-pointOnB;
+ btScalar lenSqr =m_cachedSeparatingAxis.length2();
+
+ //valid normal
+ if (lenSqr < 0.0001)
+ {
+ m_degenerateSimplex = 5;
+ }
+ if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
+ {
+ btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+ normalInB *= rlen; //normalize
+ btScalar s = btSqrt(squaredDistance);
+
+ btAssert(s > btScalar(0.0));
+ pointOnA -= m_cachedSeparatingAxis * (marginA / s);
+ pointOnB += m_cachedSeparatingAxis * (marginB / s);
+ distance = ((btScalar(1.)/rlen) - margin);
+ isValid = true;
+
+ m_lastUsedMethod = 1;
+ } else
+ {
+ m_lastUsedMethod = 2;
+ }
+ }
+
+ bool catchDegeneratePenetrationCase =
+ (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
+
+ //if (checkPenetration && !isValid)
+ if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
+ {
+ //penetration case
+
+ //if there is no way to handle penetrations, bail out
+ if (m_penetrationDepthSolver)
+ {
+ // Penetration depth case.
+ btVector3 tmpPointOnA,tmpPointOnB;
+
+ gNumDeepPenetrationChecks++;
+ m_cachedSeparatingAxis.setZero();
+
+ bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
+ *m_simplexSolver,
+ m_minkowskiA,m_minkowskiB,
+ localTransA,localTransB,
+ m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
+ debugDraw,input.m_stackAlloc
+ );
+
+
+ if (isValid2)
+ {
+ btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
+ btScalar lenSqr = tmpNormalInB.length2();
+ if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+ {
+ tmpNormalInB = m_cachedSeparatingAxis;
+ lenSqr = m_cachedSeparatingAxis.length2();
+ }
+
+ if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+ {
+ tmpNormalInB /= btSqrt(lenSqr);
+ btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
+ //only replace valid penetrations when the result is deeper (check)
+ if (!isValid || (distance2 < distance))
+ {
+ distance = distance2;
+ pointOnA = tmpPointOnA;
+ pointOnB = tmpPointOnB;
+ normalInB = tmpNormalInB;
+ isValid = true;
+ m_lastUsedMethod = 3;
+ } else
+ {
+ m_lastUsedMethod = 8;
+ }
+ } else
+ {
+ m_lastUsedMethod = 9;
+ }
+ } else
+
+ {
+ ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
+ ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
+ ///reports a valid positive distance. Use the results of the second GJK instead of failing.
+ ///thanks to Jacob.Langford for the reproduction case
+ ///http://code.google.com/p/bullet/issues/detail?id=250
+
+
+ if (m_cachedSeparatingAxis.length2() > btScalar(0.))
+ {
+ btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
+ //only replace valid distances when the distance is less
+ if (!isValid || (distance2 < distance))
+ {
+ distance = distance2;
+ pointOnA = tmpPointOnA;
+ pointOnB = tmpPointOnB;
+ pointOnA -= m_cachedSeparatingAxis * marginA ;
+ pointOnB += m_cachedSeparatingAxis * marginB ;
+ normalInB = m_cachedSeparatingAxis;
+ normalInB.normalize();
+ isValid = true;
+ m_lastUsedMethod = 6;
+ } else
+ {
+ m_lastUsedMethod = 5;
+ }
+ }
+ }
+
+ }
+
+ }
+ }
+
+
+
+ if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
+ {
+#if 0
+///some debugging
+// if (check2d)
+ {
+ printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
+ printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
+ }
+#endif
+
+ m_cachedSeparatingAxis = normalInB;
+ m_cachedSeparatingDistance = distance;
+
+ output.addContactPoint(
+ normalInB,
+ pointOnB+positionOffset,
+ distance);
+
+ }
+
+
+}
+
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btHeightfieldTerrainShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btHeightfieldTerrainShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btHeightfieldTerrainShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btHeightfieldTerrainShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,411 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
+
+#include "LinearMath/btTransformUtil.h"
+
+
+
+btHeightfieldTerrainShape::btHeightfieldTerrainShape
+(
+int heightStickWidth, int heightStickLength, void* heightfieldData,
+btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis,
+PHY_ScalarType hdt, bool flipQuadEdges
+)
+{
+ initialize(heightStickWidth, heightStickLength, heightfieldData,
+ heightScale, minHeight, maxHeight, upAxis, hdt,
+ flipQuadEdges);
+}
+
+
+
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges)
+{
+ // legacy constructor: support only float or unsigned char,
+ // and min height is zero
+ PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR;
+ btScalar minHeight = 0.0;
+
+ // previously, height = uchar * maxHeight / 65535.
+ // So to preserve legacy behavior, heightScale = maxHeight / 65535
+ btScalar heightScale = maxHeight / 65535;
+
+ initialize(heightStickWidth, heightStickLength, heightfieldData,
+ heightScale, minHeight, maxHeight, upAxis, hdt,
+ flipQuadEdges);
+}
+
+
+
+void btHeightfieldTerrainShape::initialize
+(
+int heightStickWidth, int heightStickLength, void* heightfieldData,
+btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
+PHY_ScalarType hdt, bool flipQuadEdges
+)
+{
+ // validation
+ btAssert(heightStickWidth > 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:
+ {
+ m_localAabbMin.setValue(m_minHeight, 0, 0);
+ m_localAabbMax.setValue(m_maxHeight, m_width, m_length);
+ break;
+ }
+ case 1:
+ {
+ m_localAabbMin.setValue(0, m_minHeight, 0);
+ m_localAabbMax.setValue(m_width, m_maxHeight, m_length);
+ break;
+ };
+ case 2:
+ {
+ 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 && "Bad m_upAxis");
+ }
+ }
+
+ // remember origin (defined as exact middle of aabb)
+ m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax);
+}
+
+
+
+btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
+{
+}
+
+
+
+void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5);
+
+ btVector3 localOrigin(0, 0, 0);
+ localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5);
+ localOrigin *= m_localScaling;
+
+ 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));
+ extent += btVector3(getMargin(),getMargin(),getMargin());
+
+ aabbMin = center - extent;
+ aabbMax = center + extent;
+}
+
+
+/// 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;
+ switch (m_heightDataType)
+ {
+ 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(x<m_heightStickWidth);
+ btAssert(y<m_heightStickLength);
+
+ btScalar height = getRawHeightFieldValue(x,y);
+
+ switch (m_upAxis)
+ {
+ case 0:
+ {
+ vertex.setValue(
+ height - m_localOrigin.getX(),
+ (-m_width/btScalar(2.0)) + x,
+ (-m_length/btScalar(2.0) ) + y
+ );
+ break;
+ }
+ case 1:
+ {
+ vertex.setValue(
+ (-m_width/btScalar(2.0)) + x,
+ height - m_localOrigin.getY(),
+ (-m_length/btScalar(2.0)) + y
+ );
+ break;
+ };
+ case 2:
+ {
+ vertex.setValue(
+ (-m_width/btScalar(2.0)) + x,
+ (-m_length/btScalar(2.0)) + y,
+ height - m_localOrigin.getZ()
+ );
+ break;
+ }
+ default:
+ {
+ //need to get valid m_upAxis
+ btAssert(0);
+ }
+ }
+
+ vertex*=m_localScaling;
+}
+
+
+
+static inline int
+getQuantized
+(
+btScalar x
+)
+{
+ if (x < 0.0) {
+ return (int) (x - 0.5);
+ }
+ return (int) (x + 0.5);
+}
+
+
+
+/// given input vector, return quantized version
+/**
+ This routine is basically determining the gridpoint indices for a given
+ input vector, answering the question: "which gridpoint is closest to the
+ provided point?".
+
+ "with clamp" means that we restrict the point to be in the heightfield's
+ axis-aligned bounding box.
+ */
+void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const
+{
+ btVector3 clampedPoint(point);
+ clampedPoint.setMax(m_localAabbMin);
+ clampedPoint.setMin(m_localAabbMax);
+
+ out[0] = getQuantized(clampedPoint.getX());
+ out[1] = getQuantized(clampedPoint.getY());
+ out[2] = getQuantized(clampedPoint.getZ());
+
+}
+
+
+
+/// process all triangles within the provided axis-aligned bounding box
+/**
+ basic algorithm:
+ - convert input aabb to local coordinates (scale down and shift for local origin)
+ - convert input aabb to a range of heightfield grid points (quantize)
+ - iterate over all triangles in that subset of the grid
+ */
+void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+ // scale down the input aabb's so they are in local (non-scaled) coordinates
+ btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
+ btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
+
+ // account for local origin
+ localAabbMin += m_localOrigin;
+ localAabbMax += m_localOrigin;
+
+ //quantize the aabbMin and aabbMax, and adjust the start/end ranges
+ int quantizedAabbMin[3];
+ int quantizedAabbMax[3];
+ quantizeWithClamp(quantizedAabbMin, localAabbMin,0);
+ quantizeWithClamp(quantizedAabbMax, localAabbMax,1);
+
+ // expand the min/max quantized values
+ // this is to catch the case where the input aabb falls between grid points!
+ for (int i = 0; i < 3; ++i) {
+ quantizedAabbMin[i]--;
+ quantizedAabbMax[i]++;
+ }
+
+ int startX=0;
+ int endX=m_heightStickWidth-1;
+ int startJ=0;
+ int endJ=m_heightStickLength-1;
+
+ switch (m_upAxis)
+ {
+ case 0:
+ {
+ if (quantizedAabbMin[1]>startX)
+ startX = quantizedAabbMin[1];
+ if (quantizedAabbMax[1]<endX)
+ endX = quantizedAabbMax[1];
+ if (quantizedAabbMin[2]>startJ)
+ startJ = quantizedAabbMin[2];
+ if (quantizedAabbMax[2]<endJ)
+ endJ = quantizedAabbMax[2];
+ break;
+ }
+ case 1:
+ {
+ if (quantizedAabbMin[0]>startX)
+ startX = quantizedAabbMin[0];
+ if (quantizedAabbMax[0]<endX)
+ endX = quantizedAabbMax[0];
+ if (quantizedAabbMin[2]>startJ)
+ startJ = quantizedAabbMin[2];
+ if (quantizedAabbMax[2]<endJ)
+ endJ = quantizedAabbMax[2];
+ break;
+ };
+ case 2:
+ {
+ if (quantizedAabbMin[0]>startX)
+ startX = quantizedAabbMin[0];
+ if (quantizedAabbMax[0]<endX)
+ endX = quantizedAabbMax[0];
+ if (quantizedAabbMin[1]>startJ)
+ startJ = quantizedAabbMin[1];
+ if (quantizedAabbMax[1]<endJ)
+ endJ = quantizedAabbMax[1];
+ break;
+ }
+ default:
+ {
+ //need to get valid m_upAxis
+ btAssert(0);
+ }
+ }
+
+
+
+
+ for(int j=startJ; j<endJ; j++)
+ {
+ for(int x=startX; x<endX; x++)
+ {
+ btVector3 vertices[3];
+ if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1)))
+ {
+ //first triangle
+ getVertex(x,j,vertices[0]);
+ getVertex(x+1,j,vertices[1]);
+ getVertex(x+1,j+1,vertices[2]);
+ callback->processTriangle(vertices,x,j);
+ //second triangle
+ getVertex(x,j,vertices[0]);
+ getVertex(x+1,j+1,vertices[1]);
+ getVertex(x,j+1,vertices[2]);
+ callback->processTriangle(vertices,x,j);
+ } else
+ {
+ //first triangle
+ getVertex(x,j,vertices[0]);
+ getVertex(x,j+1,vertices[1]);
+ getVertex(x+1,j,vertices[2]);
+ callback->processTriangle(vertices,x,j);
+ //second triangle
+ getVertex(x+1,j,vertices[0]);
+ getVertex(x,j+1,vertices[1]);
+ getVertex(x+1,j+1,vertices[2]);
+ callback->processTriangle(vertices,x,j);
+ }
+ }
+ }
+
+
+
+}
+
+void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const
+{
+ //moving concave objects not supported
+
+ inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+}
+
+void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling)
+{
+ m_localScaling = scaling;
+}
+const btVector3& btHeightfieldTerrainShape::getLocalScaling() const
+{
+ return m_localScaling;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btHinge2Constraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btHinge2Constraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btHinge2Constraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btHinge2Constraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+
+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 "BulletDynamics/ConstraintSolver/btHinge2Constraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+
+// constructor
+// anchor, axis1 and axis2 are in world coordinate system
+// axis1 must be orthogonal to axis2
+btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
+: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
+ m_anchor(anchor),
+ m_axis1(axis1),
+ m_axis2(axis2)
+{
+ // build frame basis
+ // 6DOF constraint uses Euler angles and to define limits
+ // it is assumed that rotational order is :
+ // Z - first, allowed limits are (-PI,PI);
+ // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
+ // used to prevent constraint from instability on poles;
+ // new position of X, allowed limits are (-PI,PI);
+ // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
+ // Build the frame in world coordinate system first
+ btVector3 zAxis = axis1.normalize();
+ btVector3 xAxis = axis2.normalize();
+ btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
+ btTransform frameInW;
+ frameInW.setIdentity();
+ frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
+ xAxis[1], yAxis[1], zAxis[1],
+ xAxis[2], yAxis[2], zAxis[2]);
+ frameInW.setOrigin(anchor);
+ // now get constraint frame in local coordinate systems
+ m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
+ m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
+ // sei limits
+ setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
+ setLinearUpperLimit(btVector3(0.f, 0.f, 1.f));
+ // like front wheels of a car
+ setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f));
+ setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f));
+ // enable suspension
+ enableSpring(2, true);
+ setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-)
+ setDamping(2, 0.01f);
+ setEquilibriumPoint();
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btHingeConstraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btHingeConstraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btHingeConstraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btHingeConstraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,813 @@
+/*
+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 "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btMinMax.h"
+#include <new>
+#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
+
+
+
+#define HINGE_USE_OBSOLETE_SOLVER false
+
+
+#ifndef __SPU__
+
+btHingeConstraint::btHingeConstraint()
+: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
+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, bool useReferenceFrameA)
+ :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
+ m_angularOnly(false),
+ m_enableAngularMotor(false),
+ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+ m_useReferenceFrameA(useReferenceFrameA)
+{
+ m_rbAFrame.getOrigin() = pivotInA;
+
+ // since no frame is given, assume this to be zero angle and just pick rb transform axis
+ btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
+
+ btVector3 rbAxisA2;
+ btScalar projection = axisInA.dot(rbAxisA1);
+ if (projection >= 1.0f - SIMD_EPSILON) {
+ rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
+ rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
+ } else if (projection <= -1.0f + SIMD_EPSILON) {
+ rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
+ rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
+ } else {
+ rbAxisA2 = axisInA.cross(rbAxisA1);
+ rbAxisA1 = rbAxisA2.cross(axisInA);
+ }
+
+ m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+ rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+ rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+
+ btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+ btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
+ 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() );
+
+ //start with free
+ m_lowerLimit = btScalar(1.0f);
+ m_upperLimit = btScalar(-1.0f);
+ m_biasFactor = 0.3f;
+ 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, 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, 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;
+
+ btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+ btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
+ btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
+
+
+ m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
+ 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(1.0f);
+ m_upperLimit = btScalar(-1.0f);
+ m_biasFactor = 0.3f;
+ 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, bool useReferenceFrameA)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+m_angularOnly(false),
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+{
+ //start with free
+ m_lowerLimit = btScalar(1.0f);
+ m_upperLimit = btScalar(-1.0f);
+ m_biasFactor = 0.3f;
+ 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, bool useReferenceFrameA)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+m_angularOnly(false),
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+{
+ ///not providing rigidbody B means implicitly using worldspace for body B
+
+ m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
+
+ //start with free
+ m_lowerLimit = btScalar(1.0f);
+ m_upperLimit = btScalar(-1.0f);
+ m_biasFactor = 0.3f;
+ m_relaxationFactor = 1.0f;
+ m_limitSoftness = 0.9f;
+ m_solveLimit = false;
+ m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+
+
+void btHingeConstraint::buildJacobian()
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ m_appliedImpulse = btScalar(0.);
+ m_accMotorImpulse = btScalar(0.);
+
+ if (!m_angularOnly)
+ {
+ 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(),
+ pivotBInW - m_rbB.getCenterOfMassPosition(),
+ normal[i],
+ m_rbA.getInvInertiaDiagLocal(),
+ 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
+
+ //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);
+
+ 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(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+
+ //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::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+
+ ///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);
+
+ //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
+
+ // 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);
+
+ btVector3 angVelA;
+ bodyA.getAngularVelocity(angVelA);
+ btVector3 angVelB;
+ bodyB.getAngularVelocity(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 normal = velrelOrthog.normalized();
+ btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+ getRigidBodyB().computeAngularImpulseDenominator(normal);
+ // scale for mass and relaxation
+ //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
+
+ 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 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));
+
+ }
+
+
+
+
+
+ // 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));
+
+ }
+ }
+
+ //apply motor
+ if (m_enableAngularMotor)
+ {
+ //todo: add limits too
+ btVector3 angularLimit(0,0,0);
+
+ btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+ btScalar projRelVel = velrel.dot(axisA);
+
+ btScalar desiredMotorVel = m_motorTargetVelocity;
+ btScalar motor_relvel = desiredMotorVel - projRelVel;
+
+ btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
+
+ // accumulated impulse clipping:
+ btScalar fMaxImpulse = m_maxMotorImpulse;
+ btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse;
+ btScalar clippedMotorImpulse = unclippedMotorImpulse;
+ if (newAccImpulse > fMaxImpulse)
+ {
+ newAccImpulse = fMaxImpulse;
+ clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
+ }
+ else if (newAccImpulse < -fMaxImpulse)
+ {
+ newAccImpulse = -fMaxImpulse;
+ clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
+ }
+ m_accMotorImpulse += clippedMotorImpulse;
+
+ bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
+ bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
+
+ }
+ }
+ }
+
+}
+
+
+#endif //__SPU__
+
+
+void btHingeConstraint::getInfo1(btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ }
+ else
+ {
+ info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
+ info->nub = 1;
+ //always add the row, to avoid computation (data is not available yet)
+ //prepare constraint
+ testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+ if(getSolveLimit() || getEnableAngularMotor())
+ {
+ info->m_numConstraintRows++; // limit 3rd anguar as well
+ info->nub--;
+ }
+
+ }
+}
+
+void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ }
+ else
+ {
+ //always add the 'limit' row, to avoid computation (data is not available yet)
+ info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
+ info->nub = 0;
+ }
+}
+
+void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+{
+ getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+}
+
+
+void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
+ ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
+ testLimit(transA,transB);
+
+ getInfo2Internal(info,transA,transB,angVelA,angVelB);
+}
+
+
+void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
+
+ btAssert(!m_useSolveConstraintObsolete);
+ int i, skip = info->rowskip;
+ // transforms in world space
+ btTransform trA = transA*m_rbAFrame;
+ btTransform trB = transB*m_rbBFrame;
+ // pivot point
+ btVector3 pivotAInW = trA.getOrigin();
+ btVector3 pivotBInW = trB.getOrigin();
+#if 0
+ if (0)
+ {
+ for (i=0;i<6;i++)
+ {
+ info->m_J1linearAxis[i*skip]=0;
+ info->m_J1linearAxis[i*skip+1]=0;
+ info->m_J1linearAxis[i*skip+2]=0;
+
+ info->m_J1angularAxis[i*skip]=0;
+ info->m_J1angularAxis[i*skip+1]=0;
+ info->m_J1angularAxis[i*skip+2]=0;
+
+ info->m_J2angularAxis[i*skip]=0;
+ info->m_J2angularAxis[i*skip+1]=0;
+ info->m_J2angularAxis[i*skip+2]=0;
+
+ info->m_constraintError[i*skip]=0.f;
+ }
+ }
+#endif //#if 0
+ // linear (all fixed)
+ info->m_J1linearAxis[0] = 1;
+ info->m_J1linearAxis[skip + 1] = 1;
+ info->m_J1linearAxis[2 * skip + 2] = 1;
+
+
+
+
+
+ btVector3 a1 = pivotAInW - transA.getOrigin();
+ {
+ btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+ btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
+ btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
+ btVector3 a1neg = -a1;
+ a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ }
+ btVector3 a2 = pivotBInW - transB.getOrigin();
+ {
+ btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+ btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
+ btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
+ a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ }
+ // linear RHS
+ btScalar k = info->fps * info->erp;
+ for(i = 0; i < 3; i++)
+ {
+ info->m_constraintError[i * skip] = 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 = angVelA.dot(ax1);
+ vel -= angVelB.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::updateRHS(btScalar timeStep)
+{
+ (void)timeStep;
+
+}
+
+
+btScalar btHingeConstraint::getHingeAngle()
+{
+ return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
+{
+ const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+ const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+ const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
+ btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+ return m_referenceSign * angle;
+}
+
+
+#if 0
+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;
+}
+#else
+
+
+void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
+{
+ // Compute limit information
+ m_hingeAngle = getHingeAngle(transA,transB);
+ m_correction = btScalar(0.);
+ m_limitSign = btScalar(0.);
+ m_solveLimit = false;
+ if (m_lowerLimit <= m_upperLimit)
+ {
+ m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, 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;
+}
+#endif
+
+static btVector3 vHinge(0, 0, btScalar(1));
+
+void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
+{
+ // convert target from body to constraint space
+ btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
+ qConstraint.normalize();
+
+ // extract "pure" hinge component
+ btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
+ btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
+ btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
+ qHinge.normalize();
+
+ // compute angular target, clamped to limits
+ btScalar targetAngle = qHinge.getAngle();
+ if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
+ {
+ qHinge = operator-(qHinge);
+ targetAngle = qHinge.getAngle();
+ }
+ if (qHinge.getZ() < 0)
+ targetAngle = -targetAngle;
+
+ setMotorTarget(targetAngle, dt);
+}
+
+void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
+{
+ if (m_lowerLimit < m_upperLimit)
+ {
+ if (targetAngle < m_lowerLimit)
+ targetAngle = m_lowerLimit;
+ else if (targetAngle > m_upperLimit)
+ targetAngle = m_upperLimit;
+ }
+
+ // compute angular velocity
+ btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+ btScalar dAngle = targetAngle - curAngle;
+ m_motorTargetVelocity = dAngle / dt;
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btKinematicCharacterController.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btKinematicCharacterController.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btKinematicCharacterController.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btKinematicCharacterController.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,547 @@
+/*
+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 "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/CollisionDispatch/btGhostObject.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "LinearMath/btDefaultMotionState.h"
+#include "BulletDynamics/Character/btKinematicCharacterController.h"
+
+static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
+
+
+// static helper method
+static btVector3
+getNormalizedVector(const btVector3& v)
+{
+ btVector3 n = v.normalized();
+ if (n.length() < SIMD_EPSILON) {
+ n.setValue(0, 0, 0);
+ }
+ return n;
+}
+
+
+///@todo Interact with dynamic objects,
+///Ride kinematicly animated platforms properly
+///More realistic (or maybe just a config option) falling
+/// -> Should integrate falling velocity manually and use that in stepDown()
+///Support jumping
+///Support ducking
+class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
+{
+public:
+ btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+ {
+ m_me = me;
+ }
+
+ virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
+ {
+ if (rayResult.m_collisionObject == m_me)
+ return 1.0;
+
+ return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
+ }
+protected:
+ btCollisionObject* m_me;
+};
+
+class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
+{
+public:
+ btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+ {
+ m_me = me;
+ }
+
+ virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
+ {
+ if (convexResult.m_hitCollisionObject == m_me)
+ return 1.0;
+
+ return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
+ }
+protected:
+ btCollisionObject* m_me;
+};
+
+/*
+ * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
+ *
+ * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
+ */
+btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
+{
+ return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
+}
+
+/*
+ * Returns the portion of 'direction' that is parallel to 'normal'
+ */
+btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
+{
+ btScalar magnitude = direction.dot(normal);
+ return normal * magnitude;
+}
+
+/*
+ * Returns the portion of 'direction' that is perpindicular to 'normal'
+ */
+btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
+{
+ return direction - parallelComponent(direction, normal);
+}
+
+btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
+{
+ m_upAxis = upAxis;
+ m_addedMargin = 0.02f;
+ m_walkDirection.setValue(0,0,0);
+ m_useGhostObjectSweepTest = true;
+ m_ghostObject = ghostObject;
+ m_stepHeight = stepHeight;
+ m_turnAngle = btScalar(0.0);
+ m_convexShape=convexShape;
+ m_useWalkDirection = true; // use walk direction by default, legacy behavior
+ m_velocityTimeInterval = 0.0;
+}
+
+btKinematicCharacterController::~btKinematicCharacterController ()
+{
+}
+
+btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
+{
+ return m_ghostObject;
+}
+
+bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
+{
+
+ bool penetration = false;
+
+ collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
+
+ m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+
+ btScalar maxPen = btScalar(0.0);
+ for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
+ {
+ m_manifoldArray.resize(0);
+
+ btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
+
+ if (collisionPair->m_algorithm)
+ collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
+
+
+ for (int j=0;j<m_manifoldArray.size();j++)
+ {
+ btPersistentManifold* manifold = m_manifoldArray[j];
+ btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
+ for (int p=0;p<manifold->getNumContacts();p++)
+ {
+ const btManifoldPoint&pt = manifold->getContactPoint(p);
+
+ if (pt.getDistance() < 0.0)
+ {
+ if (pt.getDistance() < maxPen)
+ {
+ maxPen = pt.getDistance();
+ m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+
+ }
+ m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2);
+ penetration = true;
+ } else {
+ //printf("touching %f\n", pt.getDistance());
+ }
+ }
+
+ //manifold->clearManifold();
+ }
+ }
+ btTransform newTrans = m_ghostObject->getWorldTransform();
+ newTrans.setOrigin(m_currentPosition);
+ m_ghostObject->setWorldTransform(newTrans);
+// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
+ return penetration;
+}
+
+void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
+{
+ // phase 1: up
+ btTransform start, end;
+ m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
+
+ start.setIdentity ();
+ end.setIdentity ();
+
+ /* FIXME: Handle penetration properly */
+ start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f));
+ end.setOrigin (m_targetPosition);
+
+ btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
+ callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+ callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+
+ if (m_useGhostObjectSweepTest)
+ {
+ m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
+ }
+ else
+ {
+ world->convexSweepTest (m_convexShape, start, end, callback);
+ }
+
+ if (callback.hasHit())
+ {
+ // we moved up only a fraction of the step height
+ m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+ } else {
+ m_currentStepOffset = m_stepHeight;
+ m_currentPosition = m_targetPosition;
+ }
+}
+
+void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
+{
+ btVector3 movementDirection = m_targetPosition - m_currentPosition;
+ btScalar movementLength = movementDirection.length();
+ if (movementLength>SIMD_EPSILON)
+ {
+ movementDirection.normalize();
+
+ btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
+ reflectDir.normalize();
+
+ btVector3 parallelDir, perpindicularDir;
+
+ parallelDir = parallelComponent (reflectDir, hitNormal);
+ perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
+
+ m_targetPosition = m_currentPosition;
+ if (0)//tangentMag != 0.0)
+ {
+ btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
+// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
+ m_targetPosition += parComponent;
+ }
+
+ if (normalMag != 0.0)
+ {
+ btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
+// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
+ m_targetPosition += perpComponent;
+ }
+ } else
+ {
+// printf("movementLength don't normalize a zero vector\n");
+ }
+}
+
+void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
+{
+ // printf("m_normalizedDirection=%f,%f,%f\n",
+ // m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
+ // phase 2: forward and strafe
+ btTransform start, end;
+ m_targetPosition = m_currentPosition + walkMove;
+ start.setIdentity ();
+ end.setIdentity ();
+
+ btScalar fraction = 1.0;
+ btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
+// printf("distance2=%f\n",distance2);
+
+ if (m_touchingContact)
+ {
+ if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
+ updateTargetPositionBasedOnCollision (m_touchingNormal);
+ }
+
+ int maxIter = 10;
+
+ while (fraction > btScalar(0.01) && maxIter-- > 0)
+ {
+ start.setOrigin (m_currentPosition);
+ end.setOrigin (m_targetPosition);
+
+ btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
+ callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+ callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+
+
+ btScalar margin = m_convexShape->getMargin();
+ m_convexShape->setMargin(margin + m_addedMargin);
+
+
+ if (m_useGhostObjectSweepTest)
+ {
+ m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ } else
+ {
+ collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ }
+
+ m_convexShape->setMargin(margin);
+
+
+ fraction -= callback.m_closestHitFraction;
+
+ if (callback.hasHit())
+ {
+ // we moved only a fraction
+ btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+ if (hitDistance<0.f)
+ {
+// printf("neg dist?\n");
+ }
+
+ /* If the distance is farther than the collision margin, move */
+ if (hitDistance > m_addedMargin)
+ {
+// printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+ }
+
+ updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
+ btVector3 currentDir = m_targetPosition - m_currentPosition;
+ distance2 = currentDir.length2();
+ if (distance2 > SIMD_EPSILON)
+ {
+ currentDir.normalize();
+ /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
+ if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
+ {
+ break;
+ }
+ } else
+ {
+// printf("currentDir: don't normalize a zero vector\n");
+ break;
+ }
+ } else {
+ // we moved whole way
+ m_currentPosition = m_targetPosition;
+ }
+
+ // if (callback.m_closestHitFraction == 0.f)
+ // break;
+
+ }
+}
+
+void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
+{
+ btTransform start, end;
+
+ // phase 3: down
+ btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
+ btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
+ m_targetPosition -= (step_drop + gravity_drop);
+
+ start.setIdentity ();
+ end.setIdentity ();
+
+ start.setOrigin (m_currentPosition);
+ end.setOrigin (m_targetPosition);
+
+ btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
+ callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+ callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+
+ if (m_useGhostObjectSweepTest)
+ {
+ m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ } else
+ {
+ collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ }
+
+ if (callback.hasHit())
+ {
+ // we dropped a fraction of the height -> hit floor
+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+ } else {
+ // we dropped the full height
+
+ m_currentPosition = m_targetPosition;
+ }
+}
+
+
+
+void btKinematicCharacterController::setWalkDirection
+(
+const btVector3& walkDirection
+)
+{
+ m_useWalkDirection = true;
+ m_walkDirection = walkDirection;
+ m_normalizedDirection = getNormalizedVector(m_walkDirection);
+}
+
+
+
+void btKinematicCharacterController::setVelocityForTimeInterval
+(
+const btVector3& velocity,
+btScalar timeInterval
+)
+{
+// printf("setVelocity!\n");
+// printf(" interval: %f\n", timeInterval);
+// printf(" velocity: (%f, %f, %f)\n",
+// velocity.x(), velocity.y(), velocity.z());
+
+ m_useWalkDirection = false;
+ m_walkDirection = velocity;
+ m_normalizedDirection = getNormalizedVector(m_walkDirection);
+ m_velocityTimeInterval = timeInterval;
+}
+
+
+
+void btKinematicCharacterController::reset ()
+{
+}
+
+void btKinematicCharacterController::warp (const btVector3& origin)
+{
+ btTransform xform;
+ xform.setIdentity();
+ xform.setOrigin (origin);
+ m_ghostObject->setWorldTransform (xform);
+}
+
+
+void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
+{
+
+ int numPenetrationLoops = 0;
+ m_touchingContact = false;
+ while (recoverFromPenetration (collisionWorld))
+ {
+ numPenetrationLoops++;
+ m_touchingContact = true;
+ if (numPenetrationLoops > 4)
+ {
+// printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+ break;
+ }
+ }
+
+ m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+ m_targetPosition = m_currentPosition;
+// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
+
+
+}
+
+void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
+{
+// printf("playerStep(): ");
+// printf(" dt = %f", dt);
+
+ // quick check...
+ if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
+// printf("\n");
+ return; // no motion
+ }
+
+ btTransform xform;
+ xform = m_ghostObject->getWorldTransform ();
+
+// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
+// printf("walkSpeed=%f\n",walkSpeed);
+
+ stepUp (collisionWorld);
+ if (m_useWalkDirection) {
+ stepForwardAndStrafe (collisionWorld, m_walkDirection);
+ } else {
+ //printf(" time: %f", m_velocityTimeInterval);
+ // still have some time left for moving!
+ btScalar dtMoving =
+ (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
+ m_velocityTimeInterval -= dt;
+
+ // how far will we move while we are moving?
+ btVector3 move = m_walkDirection * dtMoving;
+
+ // printf(" dtMoving: %f", dtMoving);
+
+ // okay, step
+ stepForwardAndStrafe(collisionWorld, move);
+ }
+ stepDown (collisionWorld, dt);
+
+ // printf("\n");
+
+ xform.setOrigin (m_currentPosition);
+ m_ghostObject->setWorldTransform (xform);
+}
+
+void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
+{
+ m_fallSpeed = fallSpeed;
+}
+
+void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
+{
+ m_jumpSpeed = jumpSpeed;
+}
+
+void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
+{
+ m_maxJumpHeight = maxJumpHeight;
+}
+
+bool btKinematicCharacterController::canJump () const
+{
+ return onGround();
+}
+
+void btKinematicCharacterController::jump ()
+{
+ if (!canJump())
+ return;
+
+#if 0
+ currently no jumping.
+ btTransform xform;
+ m_rigidBody->getMotionState()->getWorldTransform (xform);
+ btVector3 up = xform.getBasis()[1];
+ up.normalize ();
+ btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
+ m_rigidBody->applyCentralImpulse (up * magnitude);
+#endif
+}
+
+bool btKinematicCharacterController::onGround () const
+{
+ return true;
+}
+
+
+void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
+{
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btManifoldResult.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btManifoldResult.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btManifoldResult.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btManifoldResult.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,134 @@
+/*
+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 "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+
+///This is to allow MaterialCombiner/Custom Friction/Restitution values
+ContactAddedCallback gContactAddedCallback=0;
+
+///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+inline btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+ btScalar friction = body0->getFriction() * body1->getFriction();
+
+ const btScalar MAX_FRICTION = btScalar(10.);
+ if (friction < -MAX_FRICTION)
+ friction = -MAX_FRICTION;
+ if (friction > MAX_FRICTION)
+ friction = MAX_FRICTION;
+ return friction;
+
+}
+
+inline btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+ return body0->getRestitution() * body1->getRestitution();
+}
+
+
+
+btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* body1)
+ :m_manifoldPtr(0),
+ m_body0(body0),
+ m_body1(body1)
+#ifdef DEBUG_PART_INDEX
+ ,m_partId0(-1),
+ m_partId1(-1),
+ m_index0(-1),
+ m_index1(-1)
+#endif //DEBUG_PART_INDEX
+{
+ m_rootTransA = body0->getWorldTransform();
+ m_rootTransB = body1->getWorldTransform();
+}
+
+
+void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+{
+ btAssert(m_manifoldPtr);
+ //order in manifold needs to match
+
+ if (depth > m_manifoldPtr->getContactBreakingThreshold())
+ return;
+
+ bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+
+ btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+
+ btVector3 localA;
+ btVector3 localB;
+
+ if (isSwapped)
+ {
+ localA = m_rootTransB.invXform(pointA );
+ localB = m_rootTransA.invXform(pointInWorld);
+ } else
+ {
+ localA = m_rootTransA.invXform(pointA );
+ localB = m_rootTransB.invXform(pointInWorld);
+ }
+
+ btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+ newPt.m_positionWorldOnA = pointA;
+ newPt.m_positionWorldOnB = pointInWorld;
+
+ int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
+
+ newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);
+ newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1);
+
+ //BP mod, store contact triangles.
+ if (isSwapped)
+ {
+ newPt.m_partId0 = m_partId1;
+ newPt.m_partId1 = m_partId0;
+ newPt.m_index0 = m_index1;
+ newPt.m_index1 = m_index0;
+ } else
+ {
+ newPt.m_partId0 = m_partId0;
+ newPt.m_partId1 = m_partId1;
+ newPt.m_index0 = m_index0;
+ newPt.m_index1 = m_index1;
+ }
+ //printf("depth=%f\n",depth);
+ ///@todo, check this for any side effects
+ if (insertIndex >= 0)
+ {
+ //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
+ m_manifoldPtr->replaceContactPoint(newPt,insertIndex);
+ } else
+ {
+ insertIndex = m_manifoldPtr->addManifoldPoint(newPt);
+ }
+
+ //User can override friction and/or restitution
+ if (gContactAddedCallback &&
+ //and if either of the two bodies requires custom material
+ ((m_body0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
+ (m_body1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
+ {
+ //experimental feature info, for per-triangle material etc.
+ btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
+ btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
+ (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
+ }
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btMinkowskiPenetrationDepthSolver.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btMinkowskiPenetrationDepthSolver.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btMinkowskiPenetrationDepthSolver.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btMinkowskiPenetrationDepthSolver.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,357 @@
+/*
+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 "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#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] =
+{
+btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
+btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
+btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
+btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
+btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
+btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
+btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
+btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
+btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
+btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
+btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
+btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
+btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
+btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
+btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
+btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
+btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
+btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
+btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
+btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
+btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
+btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
+btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
+btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
+btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
+btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
+btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
+btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
+btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
+btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
+btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
+btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
+btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
+btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
+btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
+btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
+btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
+btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
+};
+
+
+bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
+ const btConvexShape* convexA,const btConvexShape* convexB,
+ const btTransform& transA,const btTransform& transB,
+ btVector3& v, btVector3& pa, btVector3& pb,
+ class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
+ )
+{
+
+ (void)stackAlloc;
+ (void)v;
+
+ bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
+
+ struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
+ {
+
+ btIntermediateResult():m_hasResult(false)
+ {
+ }
+
+ btVector3 m_normalOnBInWorld;
+ btVector3 m_pointInWorld;
+ btScalar m_depth;
+ bool m_hasResult;
+
+ virtual void setShapeIdentifiersA(int partId0,int index0)
+ {
+ (void)partId0;
+ (void)index0;
+ }
+ virtual void setShapeIdentifiersB(int partId1,int index1)
+ {
+ (void)partId1;
+ (void)index1;
+ }
+ void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+ {
+ m_normalOnBInWorld = normalOnBInWorld;
+ m_pointInWorld = pointInWorld;
+ m_depth = depth;
+ m_hasResult = true;
+ }
+ };
+
+ //just take fixed number of orientation, and sample the penetration depth in that direction
+ btScalar minProj = btScalar(BT_LARGE_FLOAT);
+ btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
+ btVector3 minA,minB;
+ 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];
+ btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+ btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+ btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+ int i;
+
+ int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+ for (i=0;i<numSampleDirections;i++)
+ {
+ btVector3 norm = sPenetrationDirections[i];
+ seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
+ seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
+ }
+
+ {
+ int numPDA = convexA->getNumPreferredPenetrationDirections();
+ if (numPDA)
+ {
+ for (int i=0;i<numPDA;i++)
+ {
+ btVector3 norm;
+ convexA->getPreferredPenetrationDirection(i,norm);
+ norm = transA.getBasis() * norm;
+ sPenetrationDirections[numSampleDirections] = norm;
+ seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+ seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+ numSampleDirections++;
+ }
+ }
+ }
+
+ {
+ int numPDB = convexB->getNumPreferredPenetrationDirections();
+ if (numPDB)
+ {
+ for (int i=0;i<numPDB;i++)
+ {
+ btVector3 norm;
+ convexB->getPreferredPenetrationDirection(i,norm);
+ norm = transB.getBasis() * norm;
+ sPenetrationDirections[numSampleDirections] = norm;
+ seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+ seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+ numSampleDirections++;
+ }
+ }
+ }
+
+
+
+
+ convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
+ convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
+
+ for (i=0;i<numSampleDirections;i++)
+ {
+ btVector3 norm = sPenetrationDirections[i];
+ if (check2d)
+ {
+ norm[2] = 0.f;
+ }
+ if (norm.length2()>0.01)
+ {
+
+ seperatingAxisInA = seperatingAxisInABatch[i];
+ seperatingAxisInB = seperatingAxisInBBatch[i];
+
+ pInA = supportVerticesABatch[i];
+ qInB = supportVerticesBBatch[i];
+
+ pWorld = transA(pInA);
+ qWorld = transB(qInB);
+ if (check2d)
+ {
+ pWorld[2] = 0.f;
+ qWorld[2] = 0.f;
+ }
+
+ w = qWorld - pWorld;
+ btScalar delta = norm.dot(w);
+ //find smallest delta
+ if (delta < minProj)
+ {
+ minProj = delta;
+ minNorm = norm;
+ minA = pWorld;
+ minB = qWorld;
+ }
+ }
+ }
+#else
+
+ int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+#ifndef __SPU__
+ {
+ int numPDA = convexA->getNumPreferredPenetrationDirections();
+ if (numPDA)
+ {
+ for (int i=0;i<numPDA;i++)
+ {
+ btVector3 norm;
+ convexA->getPreferredPenetrationDirection(i,norm);
+ norm = transA.getBasis() * norm;
+ sPenetrationDirections[numSampleDirections] = norm;
+ numSampleDirections++;
+ }
+ }
+ }
+
+ {
+ int numPDB = convexB->getNumPreferredPenetrationDirections();
+ if (numPDB)
+ {
+ for (int i=0;i<numPDB;i++)
+ {
+ btVector3 norm;
+ convexB->getPreferredPenetrationDirection(i,norm);
+ norm = transB.getBasis() * norm;
+ sPenetrationDirections[numSampleDirections] = norm;
+ numSampleDirections++;
+ }
+ }
+ }
+#endif // __SPU__
+
+ for (int i=0;i<numSampleDirections;i++)
+ {
+ const btVector3& norm = sPenetrationDirections[i];
+ seperatingAxisInA = (-norm)* transA.getBasis();
+ seperatingAxisInB = norm* transB.getBasis();
+ pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+ qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+ pWorld = transA(pInA);
+ qWorld = transB(qInB);
+ w = qWorld - pWorld;
+ btScalar delta = norm.dot(w);
+ //find smallest delta
+ if (delta < minProj)
+ {
+ minProj = delta;
+ minNorm = norm;
+ minA = pWorld;
+ minB = qWorld;
+ }
+ }
+#endif //USE_BATCHED_SUPPORT
+
+ //add the margins
+
+ minA += minNorm*convexA->getMarginNonVirtual();
+ minB -= minNorm*convexB->getMarginNonVirtual();
+ //no penetration
+ if (minProj < btScalar(0.))
+ return false;
+
+ btScalar extraSeparation = 0.5f;///scale dependent
+ minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
+
+
+
+
+
+//#define DEBUG_DRAW 1
+#ifdef DEBUG_DRAW
+ if (debugDraw)
+ {
+ btVector3 color(0,1,0);
+ debugDraw->drawLine(minA,minB,color);
+ color = btVector3 (1,1,1);
+ btVector3 vec = minB-minA;
+ btScalar prj2 = minNorm.dot(vec);
+ debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
+
+ }
+#endif //DEBUG_DRAW
+
+
+
+ btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
+
+ btScalar offsetDist = minProj;
+ btVector3 offset = minNorm * offsetDist;
+
+
+
+ btGjkPairDetector::ClosestPointInput input;
+
+ btVector3 newOrg = transA.getOrigin() + offset;
+
+ btTransform displacedTrans = transA;
+ displacedTrans.setOrigin(newOrg);
+
+ input.m_transformA = displacedTrans;
+ input.m_transformB = transB;
+ input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
+
+ btIntermediateResult res;
+ gjkdet.setCachedSeperatingAxis(-minNorm);
+ gjkdet.getClosestPoints(input,res,debugDraw);
+
+ btScalar correctedMinNorm = minProj - res.m_depth;
+
+
+ //the penetration depth is over-estimated, relax it
+ btScalar penetration_relaxation= btScalar(1.);
+ minNorm*=penetration_relaxation;
+
+
+ if (res.m_hasResult)
+ {
+
+ pa = res.m_pointInWorld - minNorm * correctedMinNorm;
+ pb = res.m_pointInWorld;
+ v = minNorm;
+
+#ifdef DEBUG_DRAW
+ if (debugDraw)
+ {
+ btVector3 color(1,0,0);
+ debugDraw->drawLine(pa,pb,color);
+ }
+#endif//DEBUG_DRAW
+
+
+ }
+ return res.m_hasResult;
+}
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btMinkowskiSumShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btMinkowskiSumShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btMinkowskiSumShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btMinkowskiSumShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,60 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+
+
+btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB)
+: 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()));
+ 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.
+ for (int i=0;i<numVectors;i++)
+ {
+ supportVerticesOut[i] = localGetSupportingVertexWithoutMargin(vectors[i]);
+ }
+
+}
+
+
+
+btScalar btMinkowskiSumShape::getMargin() const
+{
+ return m_shapeA->getMargin() + m_shapeB->getMargin();
+}
+
+
+void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ (void)mass;
+ btAssert(0);
+ inertia.setValue(0,0,0);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultiSapBroadphase.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultiSapBroadphase.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultiSapBroadphase.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultiSapBroadphase.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,489 @@
+/*
+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 "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h"
+
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h"
+
+/// btSapBroadphaseArray m_sapBroadphases;
+
+/// btOverlappingPairCache* m_overlappingPairs;
+extern int gOverlappingPairs;
+
+/*
+class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
+{
+public:
+
+ virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+ {
+ return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy);
+ }
+};
+
+*/
+
+btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache)
+:m_overlappingPairs(pairCache),
+m_optimizedAabbTree(0),
+m_ownsPairCache(false),
+m_invalidPair(0)
+{
+ if (!m_overlappingPairs)
+ {
+ m_ownsPairCache = true;
+ void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16);
+ m_overlappingPairs = new (mem)btSortedOverlappingPairCache();
+ }
+
+ struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
+ {
+ virtual ~btMultiSapOverlapFilterCallback()
+ {}
+ // return true when pairs need collision
+ virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
+ {
+ btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
+ btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
+
+ bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
+ collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
+
+ return collides;
+ }
+ };
+
+ void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16);
+ m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
+
+ m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
+// mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
+// m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
+}
+
+btMultiSapBroadphase::~btMultiSapBroadphase()
+{
+ if (m_ownsPairCache)
+ {
+ m_overlappingPairs->~btOverlappingPairCache();
+ btAlignedFree(m_overlappingPairs);
+ }
+}
+
+
+void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax)
+{
+ m_optimizedAabbTree = new btQuantizedBvh();
+ m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax);
+ QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray();
+ for (int i=0;i<m_sapBroadphases.size();i++)
+ {
+ btQuantizedBvhNode node;
+ btVector3 aabbMin,aabbMax;
+ m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax);
+ m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
+ m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
+ int partId = 0;
+ node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i;
+ nodes.push_back(node);
+ }
+ m_optimizedAabbTree->buildInternal();
+}
+
+btBroadphaseProxy* btMultiSapBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/)
+{
+ //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
+
+ void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
+ btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin, aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
+ m_multiSapProxies.push_back(proxy);
+
+ ///this should deal with inserting/removal into child broadphases
+ setAabb(proxy,aabbMin,aabbMax,dispatcher);
+ return proxy;
+}
+
+void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
+{
+ ///not yet
+ btAssert(0);
+
+}
+
+
+void btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase)
+{
+ void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16);
+ btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy;
+ bridgeProxyRef->m_childProxy = childProxy;
+ bridgeProxyRef->m_childBroadphase = childBroadphase;
+ parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef);
+}
+
+
+bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax);
+bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
+{
+return
+amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
+amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
+amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
+}
+
+
+
+
+
+
+void btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+ btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(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<m_multiSapProxies.size();i++)
+ {
+ rayCallback.process(m_multiSapProxies[i]);
+ }
+}
+
+
+//#include <stdio.h>
+
+void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
+{
+ btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
+ multiProxy->m_aabbMin = aabbMin;
+ multiProxy->m_aabbMax = aabbMax;
+
+
+// bool fullyContained = false;
+// bool alreadyInSimple = false;
+
+
+
+
+ struct MyNodeOverlapCallback : public btNodeOverlapCallback
+ {
+ btMultiSapBroadphase* m_multiSap;
+ btMultiSapProxy* m_multiProxy;
+ btDispatcher* m_dispatcher;
+
+ MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
+ :m_multiSap(multiSap),
+ m_multiProxy(multiProxy),
+ m_dispatcher(dispatcher)
+ {
+
+ }
+
+ virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex)
+ {
+ btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
+
+ int containingBroadphaseIndex = -1;
+ //already found?
+ for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
+ {
+
+ if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
+ {
+ containingBroadphaseIndex = i;
+ break;
+ }
+ }
+ if (containingBroadphaseIndex<0)
+ {
+ //add it
+ btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy);
+ m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
+
+ }
+ }
+ };
+
+ MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher);
+
+
+
+
+ if (m_optimizedAabbTree)
+ m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
+
+ int i;
+
+ for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
+ {
+ btVector3 worldAabbMin,worldAabbMax;
+ multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
+ bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
+ if (!overlapsBroadphase)
+ {
+ //remove it now
+ btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
+
+ btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
+ bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
+
+ multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
+ multiProxy->m_bridgeProxies.pop_back();
+
+ }
+ }
+
+
+ /*
+
+ if (1)
+ {
+
+ //find broadphase that contain this multiProxy
+ int numChildBroadphases = getBroadphaseArray().size();
+ for (int i=0;i<numChildBroadphases;i++)
+ {
+ btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
+ btVector3 worldAabbMin,worldAabbMax;
+ childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
+ bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
+
+ // fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
+ int containingBroadphaseIndex = -1;
+
+ //if already contains this
+
+ for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
+ {
+ if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
+ {
+ containingBroadphaseIndex = i;
+ }
+ alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
+ }
+
+ if (overlapsBroadphase)
+ {
+ if (containingBroadphaseIndex<0)
+ {
+ btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
+ childProxy->m_multiSapParentProxy = multiProxy;
+ addToChildBroadphase(multiProxy,childProxy,childBroadphase);
+ }
+ } else
+ {
+ if (containingBroadphaseIndex>=0)
+ {
+ //remove
+ btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
+
+ btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
+ bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
+
+ multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
+ multiProxy->m_bridgeProxies.pop_back();
+ }
+ }
+ }
+
+
+ ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force)
+ ///hopefully we don't end up with many entries here (can assert/provide feedback on stats)
+ if (0)//!multiProxy->m_bridgeProxies.size())
+ {
+ ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
+ ///this is needed to be able to calculate the aabb overlap
+ btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
+ childProxy->m_multiSapParentProxy = multiProxy;
+ addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
+ }
+ }
+
+ if (!multiProxy->m_bridgeProxies.size())
+ {
+ ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
+ ///this is needed to be able to calculate the aabb overlap
+ btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
+ childProxy->m_multiSapParentProxy = multiProxy;
+ addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
+ }
+*/
+
+
+ //update
+ for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
+ {
+ btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
+ bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
+ }
+
+}
+bool stopUpdating=false;
+
+
+
+class btMultiSapBroadphasePairSortPredicate
+{
+ public:
+
+ bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 )
+ {
+ btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0;
+ btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0;
+ btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0;
+ btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
+
+ return aProxy0 > bProxy0 ||
+ (aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
+ (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm);
+ }
+};
+
+
+ ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+
+// m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
+
+ if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
+ {
+
+ btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
+
+ // quicksort(overlappingPairArray,0,overlappingPairArray.size());
+
+ overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
+
+ //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+ // overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
+
+ overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+ m_invalidPair = 0;
+
+
+ int i;
+
+ btBroadphasePair previousPair;
+ previousPair.m_pProxy0 = 0;
+ previousPair.m_pProxy1 = 0;
+ previousPair.m_algorithm = 0;
+
+
+ for (i=0;i<overlappingPairArray.size();i++)
+ {
+
+ btBroadphasePair& pair = overlappingPairArray[i];
+
+ btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
+ btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0;
+ btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
+ btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
+
+ bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
+
+ previousPair = pair;
+
+ bool needsRemoval = false;
+
+ if (!isDuplicate)
+ {
+ bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+ if (hasOverlap)
+ {
+ needsRemoval = false;//callback->processOverlap(pair);
+ } else
+ {
+ needsRemoval = true;
+ }
+ } else
+ {
+ //remove duplicate
+ needsRemoval = true;
+ //should have no algorithm
+ btAssert(!pair.m_algorithm);
+ }
+
+ if (needsRemoval)
+ {
+ getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
+
+ // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+ // m_overlappingPairArray.pop_back();
+ pair.m_pProxy0 = 0;
+ pair.m_pProxy1 = 0;
+ m_invalidPair++;
+ gOverlappingPairs--;
+ }
+
+ }
+
+ ///if you don't like to skip the invalid pairs in the array, execute following code:
+ #define CLEAN_INVALID_PAIRS 1
+ #ifdef CLEAN_INVALID_PAIRS
+
+ //perform a sort, to sort 'invalid' pairs to the end
+ //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
+ overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
+
+ overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+ m_invalidPair = 0;
+ #endif//CLEAN_INVALID_PAIRS
+
+ //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
+ }
+
+
+}
+
+
+bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
+{
+ btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
+ btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
+
+ return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
+ multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
+
+}
+
+
+void btMultiSapBroadphase::printStats()
+{
+/* printf("---------------------------------\n");
+
+ printf("btMultiSapBroadphase.h\n");
+ printf("numHandles = %d\n",m_multiSapProxies.size());
+ //find broadphase that contain this multiProxy
+ int numChildBroadphases = getBroadphaseArray().size();
+ for (int i=0;i<numChildBroadphases;i++)
+ {
+
+ btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
+ childBroadphase->printStats();
+
+ }
+ */
+
+}
+
+void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
+{
+ // not yet
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultiSphereShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultiSphereShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultiSphereShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultiSphereShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,140 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+#include "LinearMath/btQuaternion.h"
+
+btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres)
+:btConvexInternalAabbCachingShape ()
+{
+ m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE;
+ //btScalar startMargin = btScalar(BT_LARGE_FLOAT);
+
+ m_localPositionArray.resize(numSpheres);
+ m_radiArray.resize(numSpheres);
+ for (int i=0;i<numSpheres;i++)
+ {
+ m_localPositionArray[i] = positions[i];
+ m_radiArray[i] = radi[i];
+
+ }
+
+ recalcLocalAabb();
+
+}
+
+
+ btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+ int i;
+ btVector3 supVec(0,0,0);
+
+ btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
+
+
+ btVector3 vec = vec0;
+ btScalar lenSqr = vec.length2();
+ if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON))
+ {
+ vec.setValue(1,0,0);
+ } else
+ {
+ btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+ vec *= rlen;
+ }
+
+ btVector3 vtx;
+ btScalar newDot;
+
+ const btVector3* pos = &m_localPositionArray[0];
+ const btScalar* rad = &m_radiArray[0];
+ int numSpheres = m_localPositionArray.size();
+
+ for (i=0;i<numSpheres;i++)
+ {
+ vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
+ pos++;
+ rad++;
+ newDot = vec.dot(vtx);
+ if (newDot > maxDot)
+ {
+ maxDot = newDot;
+ supVec = vtx;
+ }
+ }
+
+ return supVec;
+
+}
+
+ void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+
+ for (int j=0;j<numVectors;j++)
+ {
+ btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
+
+ const btVector3& vec = vectors[j];
+
+ btVector3 vtx;
+ btScalar newDot;
+
+ const btVector3* pos = &m_localPositionArray[0];
+ const btScalar* rad = &m_radiArray[0];
+ int numSpheres = m_localPositionArray.size();
+ for (int i=0;i<numSpheres;i++)
+ {
+ vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
+ pos++;
+ rad++;
+ newDot = vec.dot(vtx);
+ if (newDot > maxDot)
+ {
+ maxDot = newDot;
+ supportVerticesOut[j] = vtx;
+ }
+ }
+ }
+}
+
+
+
+
+
+
+
+
+void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ //as an approximation, take the inertia of the box that bounds the spheres
+
+ btVector3 localAabbMin,localAabbMax;
+ getCachedLocalAabb(localAabbMin,localAabbMax);
+ btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5);
+
+ btScalar lx=btScalar(2.)*(halfExtents.x());
+ btScalar ly=btScalar(2.)*(halfExtents.y());
+ btScalar lz=btScalar(2.)*(halfExtents.z());
+
+ inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
+ mass/(btScalar(12.0)) * (lx*lx + lz*lz),
+ mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultimaterialTriangleMeshShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultimaterialTriangleMeshShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultimaterialTriangleMeshShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btMultimaterialTriangleMeshShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,45 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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.
+*/
+
+/// This file was created by Alex Silverman
+
+#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h"
+//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
+
+
+///Obtains the material for a specific triangle
+const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex)
+{
+ const unsigned char * materialBase = 0;
+ int numMaterials;
+ PHY_ScalarType materialType;
+ int materialStride;
+ const unsigned char * triangleMaterialBase = 0;
+ int numTriangles;
+ int triangleMaterialStride;
+ PHY_ScalarType triangleType;
+
+ ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride,
+ &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID);
+
+ // return the pointer to the place with the friction for the triangle
+ // TODO: This depends on whether it's a moving mesh or not
+ // BUG IN GIMPACT
+ //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]);
+ int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)]));
+ btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride]));
+ return (matVal);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btOptimizedBvh.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btOptimizedBvh.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btOptimizedBvh.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btOptimizedBvh.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,391 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btOptimizedBvh.h"
+#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "LinearMath/btIDebugDraw.h"
+
+
+btOptimizedBvh::btOptimizedBvh()
+{
+}
+
+btOptimizedBvh::~btOptimizedBvh()
+{
+}
+
+
+void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax)
+{
+ m_useQuantization = useQuantizedAabbCompression;
+
+
+ // NodeArray triangleNodes;
+
+ struct NodeTriangleCallback : public btInternalTriangleIndexCallback
+ {
+
+ NodeArray& m_triangleNodes;
+
+ NodeTriangleCallback& operator=(NodeTriangleCallback& other)
+ {
+ m_triangleNodes = other.m_triangleNodes;
+ return *this;
+ }
+
+ NodeTriangleCallback(NodeArray& triangleNodes)
+ :m_triangleNodes(triangleNodes)
+ {
+ }
+
+ virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
+ {
+ btOptimizedBvhNode node;
+ btVector3 aabbMin,aabbMax;
+ aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+ aabbMin.setMin(triangle[0]);
+ aabbMax.setMax(triangle[0]);
+ aabbMin.setMin(triangle[1]);
+ aabbMax.setMax(triangle[1]);
+ aabbMin.setMin(triangle[2]);
+ aabbMax.setMax(triangle[2]);
+
+ //with quantization?
+ node.m_aabbMinOrg = aabbMin;
+ node.m_aabbMaxOrg = aabbMax;
+
+ node.m_escapeIndex = -1;
+
+ //for child nodes
+ node.m_subPart = partId;
+ node.m_triangleIndex = triangleIndex;
+ m_triangleNodes.push_back(node);
+ }
+ };
+ struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback
+ {
+ QuantizedNodeArray& m_triangleNodes;
+ const btQuantizedBvh* m_optimizedTree; // for quantization
+
+ QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other)
+ {
+ m_triangleNodes = other.m_triangleNodes;
+ m_optimizedTree = other.m_optimizedTree;
+ return *this;
+ }
+
+ QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btQuantizedBvh* tree)
+ :m_triangleNodes(triangleNodes),m_optimizedTree(tree)
+ {
+ }
+
+ virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
+ {
+ // The partId and triangle index must fit in the same (positive) integer
+ btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS));
+ btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS)));
+ //negative indices are reserved for escapeIndex
+ btAssert(triangleIndex>=0);
+
+ btQuantizedBvhNode node;
+ btVector3 aabbMin,aabbMax;
+ aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+ aabbMin.setMin(triangle[0]);
+ aabbMax.setMax(triangle[0]);
+ aabbMin.setMin(triangle[1]);
+ aabbMax.setMax(triangle[1]);
+ aabbMin.setMin(triangle[2]);
+ aabbMax.setMax(triangle[2]);
+
+ //PCK: add these checks for zero dimensions of aabb
+ const btScalar MIN_AABB_DIMENSION = btScalar(0.002);
+ const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001);
+ if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION)
+ {
+ aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION);
+ aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION);
+ }
+ if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION)
+ {
+ aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION);
+ aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION);
+ }
+ if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION)
+ {
+ aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION);
+ aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION);
+ }
+
+ m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
+ m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
+
+ node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
+
+ m_triangleNodes.push_back(node);
+ }
+ };
+
+
+
+ int numLeafNodes = 0;
+
+
+ if (m_useQuantization)
+ {
+
+ //initialize quantization values
+ setQuantizationValues(bvhAabbMin,bvhAabbMax);
+
+ QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this);
+
+
+ triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax);
+
+ //now we have an array of leafnodes in m_leafNodes
+ numLeafNodes = m_quantizedLeafNodes.size();
+
+
+ m_quantizedContiguousNodes.resize(2*numLeafNodes);
+
+
+ } else
+ {
+ NodeTriangleCallback callback(m_leafNodes);
+
+ btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+ btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+
+ triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
+
+ //now we have an array of leafnodes in m_leafNodes
+ numLeafNodes = m_leafNodes.size();
+
+ m_contiguousNodes.resize(2*numLeafNodes);
+ }
+
+ m_curNodeIndex = 0;
+
+ buildTree(0,numLeafNodes);
+
+ ///if the entire tree is small then subtree size, we need to create a header info for the tree
+ if(m_useQuantization && !m_SubtreeHeaders.size())
+ {
+ btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+ subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
+ subtree.m_rootNodeIndex = 0;
+ subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
+ }
+
+ //PCK: update the copy of the size
+ m_subtreeHeaderCount = m_SubtreeHeaders.size();
+
+ //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
+ m_quantizedLeafNodes.clear();
+ m_leafNodes.clear();
+}
+
+
+
+
+void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+ if (m_useQuantization)
+ {
+
+ setQuantizationValues(aabbMin,aabbMax);
+
+ updateBvhNodes(meshInterface,0,m_curNodeIndex,0);
+
+ ///now update all subtree headers
+
+ int i;
+ for (i=0;i<m_SubtreeHeaders.size();i++)
+ {
+ btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
+ subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
+ }
+
+ } else
+ {
+
+ }
+}
+
+
+
+
+void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+ //incrementally initialize quantization values
+ btAssert(m_useQuantization);
+
+ btAssert(aabbMin.getX() > m_bvhAabbMin.getX());
+ btAssert(aabbMin.getY() > m_bvhAabbMin.getY());
+ btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ());
+
+ btAssert(aabbMax.getX() < m_bvhAabbMax.getX());
+ btAssert(aabbMax.getY() < m_bvhAabbMax.getY());
+ btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ());
+
+ ///we should update all quantization values, using updateBvhNodes(meshInterface);
+ ///but we only update chunks that overlap the given aabb
+
+ unsigned short quantizedQueryAabbMin[3];
+ unsigned short quantizedQueryAabbMax[3];
+
+ quantize(&quantizedQueryAabbMin[0],aabbMin,0);
+ quantize(&quantizedQueryAabbMax[0],aabbMax,1);
+
+ int i;
+ for (i=0;i<this->m_SubtreeHeaders.size();i++)
+ {
+ btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
+
+ //PCK: unsigned instead of bool
+ unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+ if (overlap != 0)
+ {
+ updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i);
+
+ subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
+ }
+ }
+
+}
+
+void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index)
+{
+ (void)index;
+
+ btAssert(m_useQuantization);
+
+ int curNodeSubPart=-1;
+
+ //get access info to trianglemesh data
+ const unsigned char *vertexbase = 0;
+ int numverts = 0;
+ PHY_ScalarType type = PHY_INTEGER;
+ int stride = 0;
+ const unsigned char *indexbase = 0;
+ int indexstride = 0;
+ int numfaces = 0;
+ PHY_ScalarType indicestype = PHY_INTEGER;
+
+ btVector3 triangleVerts[3];
+ btVector3 aabbMin,aabbMax;
+ const btVector3& meshScaling = meshInterface->getScaling();
+
+ int i;
+ for (i=endNode-1;i>=firstNode;i--)
+ {
+
+
+ btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
+ if (curNode.isLeafNode())
+ {
+ //recalc aabb from triangle data
+ int nodeSubPart = curNode.getPartId();
+ int nodeTriangleIndex = curNode.getTriangleIndex();
+ if (nodeSubPart != curNodeSubPart)
+ {
+ if (curNodeSubPart >= 0)
+ meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
+ meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);
+
+ curNodeSubPart = nodeSubPart;
+ btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
+ }
+ //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
+
+ unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
+
+
+ for (int j=2;j>=0;j--)
+ {
+
+ int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+ if (type == PHY_FLOAT)
+ {
+ float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+ triangleVerts[j] = btVector3(
+ graphicsbase[0]*meshScaling.getX(),
+ graphicsbase[1]*meshScaling.getY(),
+ graphicsbase[2]*meshScaling.getZ());
+ }
+ else
+ {
+ double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
+ triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
+ }
+ }
+
+
+
+ aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+ aabbMin.setMin(triangleVerts[0]);
+ aabbMax.setMax(triangleVerts[0]);
+ aabbMin.setMin(triangleVerts[1]);
+ aabbMax.setMax(triangleVerts[1]);
+ aabbMin.setMin(triangleVerts[2]);
+ aabbMax.setMax(triangleVerts[2]);
+
+ quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0);
+ quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1);
+
+ } else
+ {
+ //combine aabb from both children
+
+ btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1];
+
+ btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] :
+ &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()];
+
+
+ {
+ for (int i=0;i<3;i++)
+ {
+ curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
+ if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i])
+ curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i];
+
+ curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
+ if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
+ curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
+ }
+ }
+ }
+
+ }
+
+ if (curNodeSubPart >= 0)
+ meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
+
+
+}
+
+///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
+btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
+{
+ btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian);
+
+ //we don't add additional data so just do a static upcast
+ return static_cast<btOptimizedBvh*>(bvh);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btOverlappingPairCache.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btOverlappingPairCache.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btOverlappingPairCache.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btOverlappingPairCache.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,633 @@
+/*
+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 "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "LinearMath/btAabbUtil2.h"
+
+#include <stdio.h>
+
+int gOverlappingPairs = 0;
+
+int gRemovePairs =0;
+int gAddedPairs =0;
+int gFindPairs =0;
+
+
+
+
+btHashedOverlappingPairCache::btHashedOverlappingPairCache():
+ m_overlapFilterCallback(0),
+ m_blockedForChanges(false),
+ m_ghostPairCallback(0)
+{
+ int initialAllocatedSize= 2;
+ m_overlappingPairArray.reserve(initialAllocatedSize);
+ growTables();
+}
+
+
+
+
+btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
+{
+}
+
+
+
+void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
+{
+ if (pair.m_algorithm)
+ {
+ {
+ pair.m_algorithm->~btCollisionAlgorithm();
+ dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+ pair.m_algorithm=0;
+ }
+ }
+}
+
+
+
+
+void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+
+ class CleanPairCallback : public btOverlapCallback
+ {
+ btBroadphaseProxy* m_cleanProxy;
+ btOverlappingPairCache* m_pairCache;
+ btDispatcher* m_dispatcher;
+
+ public:
+ CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
+ :m_cleanProxy(cleanProxy),
+ m_pairCache(pairCache),
+ m_dispatcher(dispatcher)
+ {
+ }
+ virtual bool processOverlap(btBroadphasePair& pair)
+ {
+ if ((pair.m_pProxy0 == m_cleanProxy) ||
+ (pair.m_pProxy1 == m_cleanProxy))
+ {
+ m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+ }
+ return false;
+ }
+
+ };
+
+ CleanPairCallback cleanPairs(proxy,this,dispatcher);
+
+ processAllOverlappingPairs(&cleanPairs,dispatcher);
+
+}
+
+
+
+
+void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+
+ class RemovePairCallback : public btOverlapCallback
+ {
+ btBroadphaseProxy* m_obsoleteProxy;
+
+ public:
+ RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
+ :m_obsoleteProxy(obsoleteProxy)
+ {
+ }
+ virtual bool processOverlap(btBroadphasePair& pair)
+ {
+ return ((pair.m_pProxy0 == m_obsoleteProxy) ||
+ (pair.m_pProxy1 == m_obsoleteProxy));
+ }
+
+ };
+
+
+ RemovePairCallback removeCallback(proxy);
+
+ processAllOverlappingPairs(&removeCallback,dispatcher);
+}
+
+
+
+
+
+btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+ gFindPairs++;
+ if(proxy0->m_uniqueId>proxy1->m_uniqueId)
+ btSwap(proxy0,proxy1);
+ int proxyId1 = proxy0->getUid();
+ int proxyId2 = proxy1->getUid();
+
+ /*if (proxyId1 > proxyId2)
+ btSwap(proxyId1, proxyId2);*/
+
+ int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+
+ if (hash >= m_hashTable.size())
+ {
+ return NULL;
+ }
+
+ int index = m_hashTable[hash];
+ while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+ {
+ index = m_next[index];
+ }
+
+ if (index == BT_NULL_PAIR)
+ {
+ return NULL;
+ }
+
+ btAssert(index < m_overlappingPairArray.size());
+
+ return &m_overlappingPairArray[index];
+}
+
+//#include <stdio.h>
+
+void btHashedOverlappingPairCache::growTables()
+{
+
+ int newCapacity = m_overlappingPairArray.capacity();
+
+ if (m_hashTable.size() < newCapacity)
+ {
+ //grow hashtable and next table
+ int curHashtableSize = m_hashTable.size();
+
+ m_hashTable.resize(newCapacity);
+ m_next.resize(newCapacity);
+
+
+ int i;
+
+ for (i= 0; i < newCapacity; ++i)
+ {
+ m_hashTable[i] = BT_NULL_PAIR;
+ }
+ for (i = 0; i < newCapacity; ++i)
+ {
+ m_next[i] = BT_NULL_PAIR;
+ }
+
+ for(i=0;i<curHashtableSize;i++)
+ {
+
+ const btBroadphasePair& pair = m_overlappingPairArray[i];
+ int proxyId1 = pair.m_pProxy0->getUid();
+ int proxyId2 = pair.m_pProxy1->getUid();
+ /*if (proxyId1 > proxyId2)
+ btSwap(proxyId1, proxyId2);*/
+ int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
+ m_next[i] = m_hashTable[hashValue];
+ m_hashTable[hashValue] = i;
+ }
+
+
+ }
+}
+
+btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+ if(proxy0->m_uniqueId>proxy1->m_uniqueId)
+ btSwap(proxy0,proxy1);
+ int proxyId1 = proxy0->getUid();
+ int proxyId2 = proxy1->getUid();
+
+ /*if (proxyId1 > proxyId2)
+ btSwap(proxyId1, proxyId2);*/
+
+ int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
+
+
+ btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+ if (pair != NULL)
+ {
+ return pair;
+ }
+ /*for(int i=0;i<m_overlappingPairArray.size();++i)
+ {
+ if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
+ (m_overlappingPairArray[i].m_pProxy1==proxy1))
+ {
+ printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
+ internalFindPair(proxy0, proxy1, hash);
+ }
+ }*/
+ int count = m_overlappingPairArray.size();
+ int oldCapacity = m_overlappingPairArray.capacity();
+ void* mem = &m_overlappingPairArray.expand();
+
+ //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)
+ {
+ growTables();
+ //hash with new capacity
+ hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+ }
+
+ pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
+// pair->m_pProxy0 = proxy0;
+// pair->m_pProxy1 = proxy1;
+ pair->m_algorithm = 0;
+ pair->m_internalTmpValue = 0;
+
+
+ m_next[count] = m_hashTable[hash];
+ m_hashTable[hash] = count;
+
+ return pair;
+}
+
+
+
+void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+{
+ gRemovePairs++;
+ if(proxy0->m_uniqueId>proxy1->m_uniqueId)
+ btSwap(proxy0,proxy1);
+ int proxyId1 = proxy0->getUid();
+ int proxyId2 = proxy1->getUid();
+
+ /*if (proxyId1 > proxyId2)
+ btSwap(proxyId1, proxyId2);*/
+
+ int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+
+ btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+ if (pair == NULL)
+ {
+ return 0;
+ }
+
+ cleanOverlappingPair(*pair,dispatcher);
+
+ void* userData = pair->m_internalInfo1;
+
+ btAssert(pair->m_pProxy0->getUid() == proxyId1);
+ btAssert(pair->m_pProxy1->getUid() == proxyId2);
+
+ int pairIndex = int(pair - &m_overlappingPairArray[0]);
+ btAssert(pairIndex < m_overlappingPairArray.size());
+
+ // Remove the pair from the hash table.
+ int index = m_hashTable[hash];
+ btAssert(index != BT_NULL_PAIR);
+
+ int previous = BT_NULL_PAIR;
+ while (index != pairIndex)
+ {
+ previous = index;
+ index = m_next[index];
+ }
+
+ if (previous != BT_NULL_PAIR)
+ {
+ btAssert(m_next[previous] == pairIndex);
+ m_next[previous] = m_next[pairIndex];
+ }
+ else
+ {
+ m_hashTable[hash] = m_next[pairIndex];
+ }
+
+ // We now move the last pair into spot of the
+ // pair being removed. We need to fix the hash
+ // table indices to support the move.
+
+ int lastPairIndex = m_overlappingPairArray.size() - 1;
+
+ if (m_ghostPairCallback)
+ m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
+
+ // If the removed pair is the last pair, we are done.
+ if (lastPairIndex == pairIndex)
+ {
+ m_overlappingPairArray.pop_back();
+ return userData;
+ }
+
+ // Remove the last pair from the hash table.
+ const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
+ /* missing swap here too, Nat. */
+ int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1));
+
+ index = m_hashTable[lastHash];
+ btAssert(index != BT_NULL_PAIR);
+
+ previous = BT_NULL_PAIR;
+ while (index != lastPairIndex)
+ {
+ previous = index;
+ index = m_next[index];
+ }
+
+ if (previous != BT_NULL_PAIR)
+ {
+ btAssert(m_next[previous] == lastPairIndex);
+ m_next[previous] = m_next[lastPairIndex];
+ }
+ else
+ {
+ m_hashTable[lastHash] = m_next[lastPairIndex];
+ }
+
+ // Copy the last pair into the remove pair's spot.
+ m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
+
+ // Insert the last pair into the hash table
+ m_next[pairIndex] = m_hashTable[lastHash];
+ m_hashTable[lastHash] = pairIndex;
+
+ m_overlappingPairArray.pop_back();
+
+ return userData;
+}
+//#include <stdio.h>
+
+void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+{
+
+ int i;
+
+// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
+ for (i=0;i<m_overlappingPairArray.size();)
+ {
+
+ btBroadphasePair* pair = &m_overlappingPairArray[i];
+ if (callback->processOverlap(*pair))
+ {
+ removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
+
+ gOverlappingPairs--;
+ } else
+ {
+ i++;
+ }
+ }
+}
+
+void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
+{
+ ///need to keep hashmap in sync with pair address, so rebuild all
+ btBroadphasePairArray tmpPairs;
+ int i;
+ for (i=0;i<m_overlappingPairArray.size();i++)
+ {
+ tmpPairs.push_back(m_overlappingPairArray[i]);
+ }
+
+ for (i=0;i<tmpPairs.size();i++)
+ {
+ removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher);
+ }
+
+ for (i = 0; i < m_next.size(); i++)
+ {
+ m_next[i] = BT_NULL_PAIR;
+ }
+
+ tmpPairs.quickSort(btBroadphasePairSortPredicate());
+
+ for (i=0;i<tmpPairs.size();i++)
+ {
+ addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1);
+ }
+
+
+}
+
+
+void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
+{
+ if (!hasDeferredRemoval())
+ {
+ btBroadphasePair findPair(*proxy0,*proxy1);
+
+ int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
+ if (findIndex < m_overlappingPairArray.size())
+ {
+ gOverlappingPairs--;
+ btBroadphasePair& pair = m_overlappingPairArray[findIndex];
+ void* userData = pair.m_internalInfo1;
+ cleanOverlappingPair(pair,dispatcher);
+ if (m_ghostPairCallback)
+ m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
+
+ m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
+ m_overlappingPairArray.pop_back();
+ return userData;
+ }
+ }
+
+ return 0;
+}
+
+
+
+
+
+
+
+
+btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+ //don't add overlap with own
+ 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;
+
+}
+
+///this findPair becomes really slow. Either sort the list to speedup the query, or
+///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
+///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
+///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
+ btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+ if (!needsBroadphaseCollision(proxy0,proxy1))
+ return 0;
+
+ btBroadphasePair tmpPair(*proxy0,*proxy1);
+ int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
+
+ if (findIndex < m_overlappingPairArray.size())
+ {
+ //btAssert(it != m_overlappingPairSet.end());
+ btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
+ return pair;
+ }
+ return 0;
+}
+
+
+
+
+
+
+
+
+
+
+//#include <stdio.h>
+
+void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+{
+
+ int i;
+
+ for (i=0;i<m_overlappingPairArray.size();)
+ {
+
+ btBroadphasePair* pair = &m_overlappingPairArray[i];
+ if (callback->processOverlap(*pair))
+ {
+ cleanOverlappingPair(*pair,dispatcher);
+ pair->m_pProxy0 = 0;
+ pair->m_pProxy1 = 0;
+ m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+ m_overlappingPairArray.pop_back();
+ gOverlappingPairs--;
+ } else
+ {
+ i++;
+ }
+ }
+}
+
+
+
+
+btSortedOverlappingPairCache::btSortedOverlappingPairCache():
+ m_blockedForChanges(false),
+ m_hasDeferredRemoval(true),
+ m_overlapFilterCallback(0),
+ m_ghostPairCallback(0)
+{
+ int initialAllocatedSize= 2;
+ m_overlappingPairArray.reserve(initialAllocatedSize);
+}
+
+btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
+{
+}
+
+void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
+{
+ if (pair.m_algorithm)
+ {
+ {
+ pair.m_algorithm->~btCollisionAlgorithm();
+ dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+ pair.m_algorithm=0;
+ gRemovePairs--;
+ }
+ }
+}
+
+
+void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+
+ class CleanPairCallback : public btOverlapCallback
+ {
+ btBroadphaseProxy* m_cleanProxy;
+ btOverlappingPairCache* m_pairCache;
+ btDispatcher* m_dispatcher;
+
+ public:
+ CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
+ :m_cleanProxy(cleanProxy),
+ m_pairCache(pairCache),
+ m_dispatcher(dispatcher)
+ {
+ }
+ virtual bool processOverlap(btBroadphasePair& pair)
+ {
+ if ((pair.m_pProxy0 == m_cleanProxy) ||
+ (pair.m_pProxy1 == m_cleanProxy))
+ {
+ m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+ }
+ return false;
+ }
+
+ };
+
+ CleanPairCallback cleanPairs(proxy,this,dispatcher);
+
+ processAllOverlappingPairs(&cleanPairs,dispatcher);
+
+}
+
+
+void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+
+ class RemovePairCallback : public btOverlapCallback
+ {
+ btBroadphaseProxy* m_obsoleteProxy;
+
+ public:
+ RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
+ :m_obsoleteProxy(obsoleteProxy)
+ {
+ }
+ virtual bool processOverlap(btBroadphasePair& pair)
+ {
+ return ((pair.m_pProxy0 == m_obsoleteProxy) ||
+ (pair.m_pProxy1 == m_obsoleteProxy));
+ }
+
+ };
+
+ RemovePairCallback removeCallback(proxy);
+
+ processAllOverlappingPairs(&removeCallback,dispatcher);
+}
+
+void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
+{
+ //should already be sorted
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btPersistentManifold.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btPersistentManifold.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btPersistentManifold.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btPersistentManifold.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,260 @@
+/*
+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 "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "LinearMath/btTransform.h"
+
+
+btScalar gContactBreakingThreshold = btScalar(0.02);
+ContactDestroyedCallback gContactDestroyedCallback = 0;
+ContactProcessedCallback gContactProcessedCallback = 0;
+
+
+
+btPersistentManifold::btPersistentManifold()
+:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
+m_body0(0),
+m_body1(0),
+m_cachedPoints (0),
+m_index1a(0)
+{
+}
+
+
+
+
+#ifdef DEBUG_PERSISTENCY
+#include <stdio.h>
+void btPersistentManifold::DebugPersistency()
+{
+ int i;
+ printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
+ for (i=0;i<m_cachedPoints;i++)
+ {
+ printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
+ }
+}
+#endif //DEBUG_PERSISTENCY
+
+void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
+{
+
+ void* oldPtr = pt.m_userPersistentData;
+ if (oldPtr)
+ {
+#ifdef DEBUG_PERSISTENCY
+ int i;
+ int occurance = 0;
+ for (i=0;i<m_cachedPoints;i++)
+ {
+ if (m_pointCache[i].m_userPersistentData == oldPtr)
+ {
+ occurance++;
+ if (occurance>1)
+ printf("error in clearUserCache\n");
+ }
+ }
+ btAssert(occurance<=0);
+#endif //DEBUG_PERSISTENCY
+
+ if (pt.m_userPersistentData && gContactDestroyedCallback)
+ {
+ (*gContactDestroyedCallback)(pt.m_userPersistentData);
+ pt.m_userPersistentData = 0;
+ }
+
+#ifdef DEBUG_PERSISTENCY
+ DebugPersistency();
+#endif
+ }
+
+
+}
+
+
+int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
+{
+
+ //calculate 4 possible cases areas, and take biggest area
+ //also need to keep 'deepest'
+
+ int maxPenetrationIndex = -1;
+#define KEEP_DEEPEST_POINT 1
+#ifdef KEEP_DEEPEST_POINT
+ btScalar maxPenetration = pt.getDistance();
+ for (int i=0;i<4;i++)
+ {
+ if (m_pointCache[i].getDistance() < maxPenetration)
+ {
+ maxPenetrationIndex = i;
+ maxPenetration = m_pointCache[i].getDistance();
+ }
+ }
+#endif //KEEP_DEEPEST_POINT
+
+ btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
+ if (maxPenetrationIndex != 0)
+ {
+ btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
+ btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+ btVector3 cross = a0.cross(b0);
+ res0 = cross.length2();
+ }
+ if (maxPenetrationIndex != 1)
+ {
+ btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+ btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+ btVector3 cross = a1.cross(b1);
+ res1 = cross.length2();
+ }
+
+ if (maxPenetrationIndex != 2)
+ {
+ btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+ btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
+ btVector3 cross = a2.cross(b2);
+ res2 = cross.length2();
+ }
+
+ if (maxPenetrationIndex != 3)
+ {
+ btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+ btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
+ btVector3 cross = a3.cross(b3);
+ res3 = cross.length2();
+ }
+
+ btVector4 maxvec(res0,res1,res2,res3);
+ int biggestarea = maxvec.closestAxis4();
+ return biggestarea;
+}
+
+
+int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
+{
+ btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
+ int size = getNumContacts();
+ int nearestPoint = -1;
+ for( int i = 0; i < size; i++ )
+ {
+ const btManifoldPoint &mp = m_pointCache[i];
+
+ btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
+ const btScalar distToManiPoint = diffA.dot(diffA);
+ if( distToManiPoint < shortestDist )
+ {
+ shortestDist = distToManiPoint;
+ nearestPoint = i;
+ }
+ }
+ return nearestPoint;
+}
+
+int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint)
+{
+ btAssert(validContactDistance(newPoint));
+
+ int insertIndex = getNumContacts();
+ if (insertIndex == MANIFOLD_CACHE_SIZE)
+ {
+#if MANIFOLD_CACHE_SIZE >= 4
+ //sort cache so best points come first, based on area
+ insertIndex = sortCachedPoints(newPoint);
+#else
+ insertIndex = 0;
+#endif
+ clearUserCache(m_pointCache[insertIndex]);
+
+ } else
+ {
+ m_cachedPoints++;
+
+
+ }
+ if (insertIndex<0)
+ insertIndex=0;
+
+ btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
+ m_pointCache[insertIndex] = newPoint;
+ return insertIndex;
+}
+
+btScalar btPersistentManifold::getContactBreakingThreshold() const
+{
+ return m_contactBreakingThreshold;
+}
+
+
+
+void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
+{
+ int i;
+#ifdef DEBUG_PERSISTENCY
+ printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
+ trA.getOrigin().getX(),
+ trA.getOrigin().getY(),
+ trA.getOrigin().getZ(),
+ trB.getOrigin().getX(),
+ trB.getOrigin().getY(),
+ trB.getOrigin().getZ());
+#endif //DEBUG_PERSISTENCY
+ /// first refresh worldspace positions and distance
+ for (i=getNumContacts()-1;i>=0;i--)
+ {
+ btManifoldPoint &manifoldPoint = m_pointCache[i];
+ manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
+ manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
+ manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
+ manifoldPoint.m_lifeTime++;
+ }
+
+ /// then
+ btScalar distance2d;
+ btVector3 projectedDifference,projectedPoint;
+ for (i=getNumContacts()-1;i>=0;i--)
+ {
+
+ btManifoldPoint &manifoldPoint = m_pointCache[i];
+ //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
+ if (!validContactDistance(manifoldPoint))
+ {
+ removeContactPoint(i);
+ } else
+ {
+ //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
+ projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
+ projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
+ distance2d = projectedDifference.dot(projectedDifference);
+ if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() )
+ {
+ removeContactPoint(i);
+ } else
+ {
+ //contact point processed callback
+ if (gContactProcessedCallback)
+ (*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
+ }
+ }
+ }
+#ifdef DEBUG_PERSISTENCY
+ DebugPersistency();
+#endif //
+}
+
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btPoint2PointConstraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btPoint2PointConstraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btPoint2PointConstraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btPoint2PointConstraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,242 @@
+/*
+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 "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include <new>
+
+
+
+btPoint2PointConstraint::btPoint2PointConstraint()
+: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),
+m_useSolveConstraintObsolete(false)
+{
+
+}
+
+
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_useSolveConstraintObsolete(false)
+{
+
+}
+
+void btPoint2PointConstraint::buildJacobian()
+{
+
+ ///we need it for both methods
+ {
+ m_appliedImpulse = btScalar(0.);
+
+ btVector3 normal(0,0,0);
+
+ 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(),
+ m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
+ normal,
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbA.getInvMass(),
+ m_rbB.getInvInertiaDiagLocal(),
+ m_rbB.getInvMass());
+ normal[i] = 0;
+ }
+ }
+
+
+}
+
+void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+{
+ getInfo1NonVirtual(info);
+}
+
+void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ } else
+ {
+ info->m_numConstraintRows = 3;
+ info->nub = 3;
+ }
+}
+
+
+
+
+void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+{
+ getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
+{
+ btAssert(!m_useSolveConstraintObsolete);
+
+ //retrieve matrices
+
+ // 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);
+ }
+
+
+
+ // 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]);
+ //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+ }
+
+ btScalar impulseClamp = m_setting.m_impulseClamp;//
+ for (j=0; j<3; j++)
+ {
+ if (m_setting.m_impulseClamp > 0)
+ {
+ info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
+ info->m_upperLimit[j*info->rowskip] = impulseClamp;
+ }
+ }
+
+}
+
+
+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);
+
+
+ // 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;
+ }
+ }
+
+}
+
+void btPoint2PointConstraint::updateRHS(btScalar timeStep)
+{
+ (void)timeStep;
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btPolyhedralConvexShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btPolyhedralConvexShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btPolyhedralConvexShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btPolyhedralConvexShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,193 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+
+btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape()
+{
+
+}
+
+
+btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+
+
+ btVector3 supVec(0,0,0);
+#ifndef __SPU__
+ int i;
+ btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
+
+ 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;
+
+ for (i=0;i<getNumVertices();i++)
+ {
+ getVertex(i,vtx);
+ newDot = vec.dot(vtx);
+ if (newDot > maxDot)
+ {
+ maxDot = newDot;
+ supVec = vtx;
+ }
+ }
+
+
+#endif //__SPU__
+ return supVec;
+}
+
+
+
+void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+#ifndef __SPU__
+ int i;
+
+ btVector3 vtx;
+ btScalar newDot;
+
+ for (i=0;i<numVectors;i++)
+ {
+ supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
+ }
+
+ for (int j=0;j<numVectors;j++)
+ {
+
+ const btVector3& vec = vectors[j];
+
+ for (i=0;i<getNumVertices();i++)
+ {
+ getVertex(i,vtx);
+ newDot = vec.dot(vtx);
+ if (newDot > supportVerticesOut[j][3])
+ {
+ //WARNING: don't swap next lines, the w component would get overwritten!
+ supportVerticesOut[j] = vtx;
+ supportVerticesOut[j][3] = newDot;
+ }
+ }
+ }
+#endif //__SPU__
+}
+
+
+
+void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+#ifndef __SPU__
+ //not yet, return box inertia
+
+ btScalar margin = getMargin();
+
+ btTransform ident;
+ ident.setIdentity();
+ btVector3 aabbMin,aabbMax;
+ getAabb(ident,aabbMin,aabbMax);
+ btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
+
+ btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
+ btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
+ btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
+ const btScalar x2 = lx*lx;
+ const btScalar y2 = ly*ly;
+ const btScalar z2 = lz*lz;
+ const btScalar scaledmass = mass * btScalar(0.08333333);
+
+ inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+#endif //__SPU__
+}
+
+
+
+void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling)
+{
+ btConvexInternalShape::setLocalScaling(scaling);
+ recalcLocalAabb();
+}
+
+btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape()
+:btPolyhedralConvexShape(),
+m_localAabbMin(1,1,1),
+m_localAabbMax(-1,-1,-1),
+m_isLocalAabbValid(false)
+{
+}
+
+void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
+}
+
+void btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
+{
+ m_isLocalAabbValid = true;
+
+ #if 1
+ static const btVector3 _directions[] =
+ {
+ btVector3( 1., 0., 0.),
+ btVector3( 0., 1., 0.),
+ btVector3( 0., 0., 1.),
+ btVector3( -1., 0., 0.),
+ btVector3( 0., -1., 0.),
+ btVector3( 0., 0., -1.)
+ };
+
+ btVector3 _supporting[] =
+ {
+ btVector3( 0., 0., 0.),
+ btVector3( 0., 0., 0.),
+ btVector3( 0., 0., 0.),
+ btVector3( 0., 0., 0.),
+ btVector3( 0., 0., 0.),
+ btVector3( 0., 0., 0.)
+ };
+
+ batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
+
+ for ( int i = 0; i < 3; ++i )
+ {
+ m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
+ m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
+ }
+
+ #else
+
+ for (int i=0;i<3;i++)
+ {
+ btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+ vec[i] = btScalar(1.);
+ btVector3 tmp = localGetSupportingVertex(vec);
+ m_localAabbMax[i] = tmp[i]+m_collisionMargin;
+ vec[i] = btScalar(-1.);
+ tmp = localGetSupportingVertex(vec);
+ m_localAabbMin[i] = tmp[i]-m_collisionMargin;
+ }
+ #endif
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btQuantizedBvh.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btQuantizedBvh.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btQuantizedBvh.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btQuantizedBvh.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1148 @@
+/*
+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 "BulletCollision/BroadphaseCollision/btQuantizedBvh.h"
+
+#include "LinearMath/btAabbUtil2.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define RAYAABB2
+
+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);
+}
+
+
+
+
+
+void btQuantizedBvh::buildInternal()
+{
+ ///assumes that caller filled in the m_quantizedLeafNodes
+ m_useQuantization = true;
+ int numLeafNodes = 0;
+
+ if (m_useQuantization)
+ {
+ //now we have an array of leafnodes in m_leafNodes
+ numLeafNodes = m_quantizedLeafNodes.size();
+
+ m_quantizedContiguousNodes.resize(2*numLeafNodes);
+
+ }
+
+ m_curNodeIndex = 0;
+
+ buildTree(0,numLeafNodes);
+
+ ///if the entire tree is small then subtree size, we need to create a header info for the tree
+ if(m_useQuantization && !m_SubtreeHeaders.size())
+ {
+ btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+ subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
+ subtree.m_rootNodeIndex = 0;
+ subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
+ }
+
+ //PCK: update the copy of the size
+ m_subtreeHeaderCount = m_SubtreeHeaders.size();
+
+ //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
+ m_quantizedLeafNodes.clear();
+ m_leafNodes.clear();
+}
+
+
+
+///just for debugging, to visualize the individual patches/subtrees
+#ifdef DEBUG_PATCH_COLORS
+btVector3 color[4]=
+{
+ btVector3(255,0,0),
+ btVector3(0,255,0),
+ btVector3(0,0,255),
+ btVector3(0,255,255)
+};
+#endif //DEBUG_PATCH_COLORS
+
+
+
+void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin)
+{
+ //enlarge the AABB to avoid division by zero when initializing the quantization values
+ btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
+ m_bvhAabbMin = bvhAabbMin - clampValue;
+ m_bvhAabbMax = bvhAabbMax + clampValue;
+ btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin;
+ m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize;
+ m_useQuantization = true;
+}
+
+
+
+
+btQuantizedBvh::~btQuantizedBvh()
+{
+}
+
+#ifdef DEBUG_TREE_BUILDING
+int gStackDepth = 0;
+int gMaxStackDepth = 0;
+#endif //DEBUG_TREE_BUILDING
+
+void btQuantizedBvh::buildTree (int startIndex,int endIndex)
+{
+#ifdef DEBUG_TREE_BUILDING
+ gStackDepth++;
+ if (gStackDepth > gMaxStackDepth)
+ gMaxStackDepth = gStackDepth;
+#endif //DEBUG_TREE_BUILDING
+
+
+ int splitAxis, splitIndex, i;
+ int numIndices =endIndex-startIndex;
+ int curIndex = m_curNodeIndex;
+
+ btAssert(numIndices>0);
+
+ if (numIndices==1)
+ {
+#ifdef DEBUG_TREE_BUILDING
+ gStackDepth--;
+#endif //DEBUG_TREE_BUILDING
+
+ assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex);
+
+ m_curNodeIndex++;
+ return;
+ }
+ //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+ splitAxis = calcSplittingAxis(startIndex,endIndex);
+
+ splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis);
+
+ int internalNodeIndex = m_curNodeIndex;
+
+ //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;i<endIndex;i++)
+ {
+ mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i));
+ }
+
+ m_curNodeIndex++;
+
+
+ //internalNode->m_escapeIndex;
+
+ int leftChildNodexIndex = m_curNodeIndex;
+
+ //build left child tree
+ buildTree(startIndex,splitIndex);
+
+ int rightChildNodexIndex = m_curNodeIndex;
+ //build right child tree
+ buildTree(splitIndex,endIndex);
+
+#ifdef DEBUG_TREE_BUILDING
+ gStackDepth--;
+#endif //DEBUG_TREE_BUILDING
+
+ int escapeIndex = m_curNodeIndex - curIndex;
+
+ if (m_useQuantization)
+ {
+ //escapeIndex is the number of nodes of this subtree
+ const int sizeQuantizedNode =sizeof(btQuantizedBvhNode);
+ const int treeSizeInBytes = escapeIndex * sizeQuantizedNode;
+ if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES)
+ {
+ updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex);
+ }
+ } else
+ {
+
+ }
+
+ setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex);
+
+}
+
+void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex)
+{
+ btAssert(m_useQuantization);
+
+ btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex];
+ int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex();
+ int leftSubTreeSizeInBytes = leftSubTreeSize * static_cast<int>(sizeof(btQuantizedBvhNode));
+
+ btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex];
+ int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex();
+ int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast<int>(sizeof(btQuantizedBvhNode));
+
+ if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
+ {
+ btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+ subtree.setAabbFromQuantizeNode(leftChildNode);
+ subtree.m_rootNodeIndex = leftChildNodexIndex;
+ subtree.m_subtreeSize = leftSubTreeSize;
+ }
+
+ if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
+ {
+ btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+ subtree.setAabbFromQuantizeNode(rightChildNode);
+ subtree.m_rootNodeIndex = rightChildNodexIndex;
+ subtree.m_subtreeSize = rightSubTreeSize;
+ }
+
+ //PCK: update the copy of the size
+ m_subtreeHeaderCount = m_SubtreeHeaders.size();
+}
+
+
+int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis)
+{
+ int i;
+ int splitIndex =startIndex;
+ int numIndices = endIndex - startIndex;
+ btScalar splitValue;
+
+ btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
+ means+=center;
+ }
+ means *= (btScalar(1.)/(btScalar)numIndices);
+
+ splitValue = means[splitAxis];
+
+ //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
+ if (center[splitAxis] > splitValue)
+ {
+ //swap
+ swapLeafNodes(i,splitIndex);
+ splitIndex++;
+ }
+ }
+
+ //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+ //otherwise the tree-building might fail due to stack-overflows in certain cases.
+ //unbalanced1 is unsafe: it can cause stack overflows
+ //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+ //unbalanced2 should work too: always use center (perfect balanced trees)
+ //bool unbalanced2 = true;
+
+ //this should be safe too:
+ int rangeBalancedIndices = numIndices/3;
+ bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+ if (unbalanced)
+ {
+ splitIndex = startIndex+ (numIndices>>1);
+ }
+
+ bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
+ (void)unbal;
+ btAssert(!unbal);
+
+ return splitIndex;
+}
+
+
+int btQuantizedBvh::calcSplittingAxis(int startIndex,int endIndex)
+{
+ int i;
+
+ btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+ btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+ int numIndices = endIndex-startIndex;
+
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
+ means+=center;
+ }
+ means *= (btScalar(1.)/(btScalar)numIndices);
+
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
+ btVector3 diff2 = center-means;
+ diff2 = diff2 * diff2;
+ variance += diff2;
+ }
+ variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );
+
+ return variance.maxAxis();
+}
+
+
+
+void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+ //either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
+
+ if (m_useQuantization)
+ {
+ ///quantize query AABB
+ unsigned short int quantizedQueryAabbMin[3];
+ unsigned short int quantizedQueryAabbMax[3];
+ quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0);
+ quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1);
+
+ switch (m_traversalMode)
+ {
+ case TRAVERSAL_STACKLESS:
+ walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,0,m_curNodeIndex);
+ break;
+ case TRAVERSAL_STACKLESS_CACHE_FRIENDLY:
+ walkStacklessQuantizedTreeCacheFriendly(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+ break;
+ case TRAVERSAL_RECURSIVE:
+ {
+ const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
+ walkRecursiveQuantizedTreeAgainstQueryAabb(rootNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+ }
+ break;
+ default:
+ //unsupported
+ btAssert(0);
+ }
+ } else
+ {
+ walkStacklessTree(nodeCallback,aabbMin,aabbMax);
+ }
+}
+
+
+int maxIterations = 0;
+
+
+void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) 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;
+
+ while (curIndex < m_curNodeIndex)
+ {
+ //catch bugs in tree data
+ btAssert (walkIterations < m_curNodeIndex);
+
+ walkIterations++;
+ aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
+ isLeafNode = rootNode->m_escapeIndex == -1;
+
+ //PCK: unsigned instead of bool
+ if (isLeafNode && (aabbOverlap != 0))
+ {
+ nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
+ }
+
+ //PCK: unsigned instead of bool
+ if ((aabbOverlap != 0) || isLeafNode)
+ {
+ rootNode++;
+ curIndex++;
+ } else
+ {
+ escapeIndex = rootNode->m_escapeIndex;
+ rootNode += escapeIndex;
+ curIndex += escapeIndex;
+ }
+ }
+ if (maxIterations < walkIterations)
+ maxIterations = walkIterations;
+
+}
+
+/*
+///this was the original recursive traversal, before we optimized towards stackless traversal
+void btQuantizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+ bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
+ if (aabbOverlap)
+ {
+ isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
+ if (isLeafNode)
+ {
+ nodeCallback->processNode(rootNode);
+ } else
+ {
+ walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax);
+ walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax);
+ }
+ }
+
+}
+*/
+
+void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const
+{
+ btAssert(m_useQuantization);
+
+ bool isLeafNode;
+ //PCK: unsigned instead of bool
+ unsigned aabbOverlap;
+
+ //PCK: unsigned instead of bool
+ aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax);
+ isLeafNode = currentNode->isLeafNode();
+
+ //PCK: unsigned instead of bool
+ if (aabbOverlap != 0)
+ {
+ if (isLeafNode)
+ {
+ nodeCallback->processNode(currentNode->getPartId(),currentNode->getTriangleIndex());
+ } else
+ {
+ //process left and right children
+ const btQuantizedBvhNode* leftChildNode = currentNode+1;
+ walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+
+ const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex();
+ walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+ }
+ }
+}
+
+
+
+void 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(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+ rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+ rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : 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
+{
+ btAssert(m_useQuantization);
+
+ int curIndex = startNodeIndex;
+ int walkIterations = 0;
+ int subTreeSize = endNodeIndex - startNodeIndex;
+ (void)subTreeSize;
+
+ const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex];
+ int escapeIndex;
+
+ bool isLeafNode;
+ //PCK: unsigned instead of bool
+ unsigned boxBoxOverlap = 0;
+ unsigned rayBoxOverlap = 0;
+
+ btScalar lambda_max = 1.0;
+
+#ifdef RAYAABB2
+ btVector3 rayDirection = (rayTarget-raySource);
+ rayDirection.normalize ();
+ lambda_max = rayDirection.dot(rayTarget-raySource);
+ ///what about division by zero? --> just set rayDirection[i] to 1.0
+ rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0];
+ rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[1];
+ rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[2];
+ unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0};
+#endif
+
+ /* 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;
+
+ unsigned short int quantizedQueryAabbMin[3];
+ unsigned short int quantizedQueryAabbMax[3];
+ quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0);
+ quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1);
+
+ while (curIndex < endNodeIndex)
+ {
+
+//#define VISUALLY_ANALYZE_BVH 1
+#ifdef VISUALLY_ANALYZE_BVH
+ //some code snippet to debugDraw aabb, to visually analyze bvh structure
+ static int drawPatch = 0;
+ //need some global access to a debugDrawer
+ extern btIDebugDraw* debugDrawerPtr;
+ if (curIndex==drawPatch)
+ {
+ btVector3 aabbMin,aabbMax;
+ aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
+ aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
+ btVector3 color(1,0,0);
+ debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
+ }
+#endif//VISUALLY_ANALYZE_BVH
+
+ //catch bugs in tree data
+ btAssert (walkIterations < subTreeSize);
+
+ walkIterations++;
+ //PCK: unsigned instead of bool
+ // only interested if this is closer than any previous hit
+ btScalar param = 1.0;
+ rayBoxOverlap = 0;
+ boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+ isLeafNode = rootNode->isLeafNode();
+ if (boxBoxOverlap)
+ {
+ btVector3 bounds[2];
+ bounds[0] = unQuantize(rootNode->m_quantizedAabbMin);
+ bounds[1] = unQuantize(rootNode->m_quantizedAabbMax);
+ /* Add box cast extents */
+ bounds[0] += aabbMin;
+ bounds[1] += aabbMax;
+ btVector3 normal;
+#if 0
+ bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max);
+ bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal);
+ if (ra2 != ra)
+ {
+ printf("functions don't match\n");
+ }
+#endif
+#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
+
+ //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
+ }
+
+ if (isLeafNode && rayBoxOverlap)
+ {
+ nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex());
+ }
+
+ //PCK: unsigned instead of bool
+ if ((rayBoxOverlap != 0) || isLeafNode)
+ {
+ rootNode++;
+ curIndex++;
+ } else
+ {
+ escapeIndex = rootNode->getEscapeIndex();
+ rootNode += escapeIndex;
+ curIndex += escapeIndex;
+ }
+ }
+ if (maxIterations < walkIterations)
+ maxIterations = walkIterations;
+
+}
+
+void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const
+{
+ btAssert(m_useQuantization);
+
+ int curIndex = startNodeIndex;
+ int walkIterations = 0;
+ int subTreeSize = endNodeIndex - startNodeIndex;
+ (void)subTreeSize;
+
+ const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex];
+ int escapeIndex;
+
+ bool isLeafNode;
+ //PCK: unsigned instead of bool
+ unsigned aabbOverlap;
+
+ while (curIndex < endNodeIndex)
+ {
+
+//#define VISUALLY_ANALYZE_BVH 1
+#ifdef VISUALLY_ANALYZE_BVH
+ //some code snippet to debugDraw aabb, to visually analyze bvh structure
+ static int drawPatch = 0;
+ //need some global access to a debugDrawer
+ extern btIDebugDraw* debugDrawerPtr;
+ if (curIndex==drawPatch)
+ {
+ btVector3 aabbMin,aabbMax;
+ aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
+ aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
+ btVector3 color(1,0,0);
+ debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
+ }
+#endif//VISUALLY_ANALYZE_BVH
+
+ //catch bugs in tree data
+ btAssert (walkIterations < subTreeSize);
+
+ walkIterations++;
+ //PCK: unsigned instead of bool
+ aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+ isLeafNode = rootNode->isLeafNode();
+
+ if (isLeafNode && aabbOverlap)
+ {
+ nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex());
+ }
+
+ //PCK: unsigned instead of bool
+ if ((aabbOverlap != 0) || isLeafNode)
+ {
+ rootNode++;
+ curIndex++;
+ } else
+ {
+ escapeIndex = rootNode->getEscapeIndex();
+ rootNode += escapeIndex;
+ curIndex += escapeIndex;
+ }
+ }
+ if (maxIterations < walkIterations)
+ maxIterations = walkIterations;
+
+}
+
+//This traversal can be called from Playstation 3 SPU
+void btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const
+{
+ btAssert(m_useQuantization);
+
+ int i;
+
+
+ for (i=0;i<this->m_SubtreeHeaders.size();i++)
+ {
+ const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
+
+ //PCK: unsigned instead of bool
+ unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+ if (overlap != 0)
+ {
+ walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
+ subtree.m_rootNodeIndex,
+ subtree.m_rootNodeIndex+subtree.m_subtreeSize);
+ }
+ }
+}
+
+
+void btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const
+{
+ 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
+{
+ //always use stackless
+
+ if (m_useQuantization)
+ {
+ walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
+ }
+ else
+ {
+ walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
+ }
+ /*
+ {
+ //recursive traversal
+ btVector3 qaabbMin = raySource;
+ btVector3 qaabbMax = raySource;
+ qaabbMin.setMin(rayTarget);
+ qaabbMax.setMax(rayTarget);
+ qaabbMin += aabbMin;
+ qaabbMax += aabbMax;
+ reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax);
+ }
+ */
+
+}
+
+
+void btQuantizedBvh::swapLeafNodes(int i,int splitIndex)
+{
+ if (m_useQuantization)
+ {
+ btQuantizedBvhNode tmp = m_quantizedLeafNodes[i];
+ m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex];
+ m_quantizedLeafNodes[splitIndex] = tmp;
+ } else
+ {
+ btOptimizedBvhNode tmp = m_leafNodes[i];
+ m_leafNodes[i] = m_leafNodes[splitIndex];
+ m_leafNodes[splitIndex] = tmp;
+ }
+}
+
+void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex)
+{
+ if (m_useQuantization)
+ {
+ m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex];
+ } else
+ {
+ m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex];
+ }
+}
+
+//PCK: include
+#include <new>
+
+#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()
+{
+ // I changed this to 0 since the extra padding is not needed or used.
+ return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT;
+}
+
+unsigned btQuantizedBvh::calculateSerializeBufferSize()
+{
+ unsigned baseSize = sizeof(btQuantizedBvh) + getAlignmentSerializationPadding();
+ baseSize += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount;
+ if (m_useQuantization)
+ {
+ return baseSize + m_curNodeIndex * sizeof(btQuantizedBvhNode);
+ }
+ return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode);
+}
+
+bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian)
+{
+ 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))
+ {
+ ///check alignedment for buffer?
+ btAssert(0);
+ return false;
+ }
+*/
+
+ btQuantizedBvh *targetBvh = (btQuantizedBvh *)o_alignedDataBuffer;
+
+ // construct the class so the virtual function table, etc will be set up
+ // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor
+ new (targetBvh) btQuantizedBvh;
+
+ if (i_swapEndian)
+ {
+ targetBvh->m_curNodeIndex = static_cast<int>(btSwapEndian(m_curNodeIndex));
+
+
+ btSwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin);
+ btSwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax);
+ btSwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization);
+
+ targetBvh->m_traversalMode = (btTraversalMode)btSwapEndian(m_traversalMode);
+ targetBvh->m_subtreeHeaderCount = static_cast<int>(btSwapEndian(m_subtreeHeaderCount));
+ }
+ else
+ {
+ targetBvh->m_curNodeIndex = m_curNodeIndex;
+ targetBvh->m_bvhAabbMin = m_bvhAabbMin;
+ targetBvh->m_bvhAabbMax = m_bvhAabbMax;
+ targetBvh->m_bvhQuantization = m_bvhQuantization;
+ targetBvh->m_traversalMode = m_traversalMode;
+ targetBvh->m_subtreeHeaderCount = m_subtreeHeaderCount;
+ }
+
+ targetBvh->m_useQuantization = m_useQuantization;
+
+ unsigned char *nodeData = (unsigned char *)targetBvh;
+ nodeData += sizeof(btQuantizedBvh);
+
+ unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+ nodeData += sizeToAdd;
+
+ int nodeCount = m_curNodeIndex;
+
+ if (m_useQuantization)
+ {
+ targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
+
+ if (i_swapEndian)
+ {
+ for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+ {
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]);
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]);
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]);
+
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]);
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]);
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]);
+
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast<int>(btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex));
+ }
+ }
+ else
+ {
+ for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+ {
+
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0];
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1];
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2];
+
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0];
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1];
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2];
+
+ targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex;
+
+
+ }
+ }
+ 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
+ {
+ targetBvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
+
+ if (i_swapEndian)
+ {
+ for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+ {
+ btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMinOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg);
+ btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMaxOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg);
+
+ targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast<int>(btSwapEndian(m_contiguousNodes[nodeIndex].m_escapeIndex));
+ targetBvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast<int>(btSwapEndian(m_contiguousNodes[nodeIndex].m_subPart));
+ targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast<int>(btSwapEndian(m_contiguousNodes[nodeIndex].m_triangleIndex));
+ }
+ }
+ else
+ {
+ for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+ {
+ targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg = m_contiguousNodes[nodeIndex].m_aabbMinOrg;
+ targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg = m_contiguousNodes[nodeIndex].m_aabbMaxOrg;
+
+ targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = m_contiguousNodes[nodeIndex].m_escapeIndex;
+ targetBvh->m_contiguousNodes[nodeIndex].m_subPart = m_contiguousNodes[nodeIndex].m_subPart;
+ targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = m_contiguousNodes[nodeIndex].m_triangleIndex;
+ }
+ }
+ 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;
+ nodeData += sizeToAdd;
+
+ // Now serialize the subtree headers
+ targetBvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, m_subtreeHeaderCount, m_subtreeHeaderCount);
+ if (i_swapEndian)
+ {
+ for (int i = 0; i < m_subtreeHeaderCount; i++)
+ {
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[0]);
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[1]);
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[2]);
+
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[0]);
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[1]);
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[2]);
+
+ targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast<int>(btSwapEndian(m_SubtreeHeaders[i].m_rootNodeIndex));
+ targetBvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast<int>(btSwapEndian(m_SubtreeHeaders[i].m_subtreeSize));
+ }
+ }
+ else
+ {
+ for (int i = 0; i < m_subtreeHeaderCount; i++)
+ {
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = (m_SubtreeHeaders[i].m_quantizedAabbMin[0]);
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = (m_SubtreeHeaders[i].m_quantizedAabbMin[1]);
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = (m_SubtreeHeaders[i].m_quantizedAabbMin[2]);
+
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = (m_SubtreeHeaders[i].m_quantizedAabbMax[0]);
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = (m_SubtreeHeaders[i].m_quantizedAabbMax[1]);
+ targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = (m_SubtreeHeaders[i].m_quantizedAabbMax[2]);
+
+ targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = (m_SubtreeHeaders[i].m_rootNodeIndex);
+ targetBvh->m_SubtreeHeaders[i].m_subtreeSize = (m_SubtreeHeaders[i].m_subtreeSize);
+
+ // 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;
+}
+
+btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
+{
+
+ if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
+ {
+ return NULL;
+ }
+ btQuantizedBvh *bvh = (btQuantizedBvh *)i_alignedDataBuffer;
+
+ if (i_swapEndian)
+ {
+ bvh->m_curNodeIndex = static_cast<int>(btSwapEndian(bvh->m_curNodeIndex));
+
+ btUnSwapVector3Endian(bvh->m_bvhAabbMin);
+ btUnSwapVector3Endian(bvh->m_bvhAabbMax);
+ btUnSwapVector3Endian(bvh->m_bvhQuantization);
+
+ bvh->m_traversalMode = (btTraversalMode)btSwapEndian(bvh->m_traversalMode);
+ bvh->m_subtreeHeaderCount = static_cast<int>(btSwapEndian(bvh->m_subtreeHeaderCount));
+ }
+
+ unsigned int calculatedBufSize = bvh->calculateSerializeBufferSize();
+ btAssert(calculatedBufSize <= i_dataBufferSize);
+
+ if (calculatedBufSize > i_dataBufferSize)
+ {
+ return NULL;
+ }
+
+ unsigned char *nodeData = (unsigned char *)bvh;
+ nodeData += sizeof(btQuantizedBvh);
+
+ unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+ nodeData += sizeToAdd;
+
+ int nodeCount = bvh->m_curNodeIndex;
+
+ // Must call placement new to fill in virtual function table, etc, but we don't want to overwrite most data, so call a special version of the constructor
+ // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor
+ new (bvh) btQuantizedBvh(*bvh, false);
+
+ if (bvh->m_useQuantization)
+ {
+ bvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
+
+ if (i_swapEndian)
+ {
+ for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+ {
+ bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]);
+ bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]);
+ bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]);
+
+ bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]);
+ bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]);
+ bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]);
+
+ bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast<int>(btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex));
+ }
+ }
+ nodeData += sizeof(btQuantizedBvhNode) * nodeCount;
+ }
+ else
+ {
+ bvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
+
+ if (i_swapEndian)
+ {
+ for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+ {
+ btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg);
+ btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg);
+
+ bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_escapeIndex));
+ bvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_subPart));
+ bvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_triangleIndex));
+ }
+ }
+ nodeData += sizeof(btOptimizedBvhNode) * nodeCount;
+ }
+
+ sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+ nodeData += sizeToAdd;
+
+ // Now serialize the subtree headers
+ bvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, bvh->m_subtreeHeaderCount, bvh->m_subtreeHeaderCount);
+ if (i_swapEndian)
+ {
+ for (int i = 0; i < bvh->m_subtreeHeaderCount; i++)
+ {
+ bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0]);
+ bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1]);
+ bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2]);
+
+ bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0]);
+ bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1]);
+ bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2]);
+
+ bvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast<int>(btSwapEndian(bvh->m_SubtreeHeaders[i].m_rootNodeIndex));
+ bvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast<int>(btSwapEndian(bvh->m_SubtreeHeaders[i].m_subtreeSize));
+ }
+ }
+
+ return bvh;
+}
+
+// Constructor that prevents btVector3's default constructor from being called
+btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) :
+m_bvhAabbMin(self.m_bvhAabbMin),
+m_bvhAabbMax(self.m_bvhAabbMax),
+m_bvhQuantization(self.m_bvhQuantization),
+m_bulletVersion(BT_BULLET_VERSION)
+{
+
+}
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btQuickprof.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btQuickprof.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btQuickprof.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btQuickprof.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,346 @@
+/*
+
+/***************************************************************************************************
+**
+** profile.cpp
+**
+** Real-Time Hierarchical Profiling for Game Programming Gems 3
+**
+** by Greg Hjelstrom & Byon Garrabrant
+**
+***************************************************************************************************/
+
+// Credits: The Clock class was inspired by the Timer classes in
+// Ogre (www.ogre3d.org).
+
+#include "LinearMath/btQuickprof.h"
+
+
+#ifdef USE_BT_CLOCK
+
+static btClock gProfileClock;
+
+inline void Profile_Get_Ticks(unsigned long int * ticks)
+{
+ *ticks = gProfileClock.getTimeMicroseconds();
+}
+
+inline float Profile_Get_Tick_Rate(void)
+{
+// return 1000000.f;
+ return 1000.f;
+
+}
+
+
+
+/***************************************************************************************************
+**
+** CProfileNode
+**
+***************************************************************************************************/
+
+/***********************************************************************************************
+ * INPUT: *
+ * name - pointer to a static string which is the name of this profile node *
+ * parent - parent pointer *
+ * *
+ * WARNINGS: *
+ * The name is assumed to be a static pointer, only the pointer is stored and compared for *
+ * efficiency reasons. *
+ *=============================================================================================*/
+CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) :
+ Name( name ),
+ TotalCalls( 0 ),
+ TotalTime( 0 ),
+ StartTime( 0 ),
+ RecursionCounter( 0 ),
+ Parent( parent ),
+ Child( NULL ),
+ Sibling( NULL )
+{
+ Reset();
+}
+
+
+void CProfileNode::CleanupMemory()
+{
+ delete ( Child);
+ Child = NULL;
+ delete ( Sibling);
+ Sibling = NULL;
+}
+
+CProfileNode::~CProfileNode( void )
+{
+ delete ( Child);
+ delete ( Sibling);
+}
+
+
+/***********************************************************************************************
+ * INPUT: *
+ * name - static string pointer to the name of the node we are searching for *
+ * *
+ * WARNINGS: *
+ * All profile names are assumed to be static strings so this function uses pointer compares *
+ * to find the named node. *
+ *=============================================================================================*/
+CProfileNode * CProfileNode::Get_Sub_Node( const char * name )
+{
+ // Try to find this sub node
+ CProfileNode * child = Child;
+ while ( child ) {
+ if ( child->Name == name ) {
+ return child;
+ }
+ child = child->Sibling;
+ }
+
+ // We didn't find it, so add it
+
+ CProfileNode * node = new CProfileNode( name, this );
+ node->Sibling = Child;
+ Child = node;
+ return node;
+}
+
+
+void CProfileNode::Reset( void )
+{
+ TotalCalls = 0;
+ TotalTime = 0.0f;
+
+
+ if ( Child ) {
+ Child->Reset();
+ }
+ if ( Sibling ) {
+ Sibling->Reset();
+ }
+}
+
+
+void CProfileNode::Call( void )
+{
+ TotalCalls++;
+ if (RecursionCounter++ == 0) {
+ Profile_Get_Ticks(&StartTime);
+ }
+}
+
+
+bool CProfileNode::Return( void )
+{
+ if ( --RecursionCounter == 0 && TotalCalls != 0 ) {
+ unsigned long int time;
+ Profile_Get_Ticks(&time);
+ time-=StartTime;
+ TotalTime += (float)time / Profile_Get_Tick_Rate();
+ }
+ return ( RecursionCounter == 0 );
+}
+
+
+/***************************************************************************************************
+**
+** CProfileIterator
+**
+***************************************************************************************************/
+CProfileIterator::CProfileIterator( CProfileNode * start )
+{
+ CurrentParent = start;
+ CurrentChild = CurrentParent->Get_Child();
+}
+
+
+void CProfileIterator::First(void)
+{
+ CurrentChild = CurrentParent->Get_Child();
+}
+
+
+void CProfileIterator::Next(void)
+{
+ CurrentChild = CurrentChild->Get_Sibling();
+}
+
+
+bool CProfileIterator::Is_Done(void)
+{
+ return CurrentChild == NULL;
+}
+
+
+void CProfileIterator::Enter_Child( int index )
+{
+ CurrentChild = CurrentParent->Get_Child();
+ while ( (CurrentChild != NULL) && (index != 0) ) {
+ index--;
+ CurrentChild = CurrentChild->Get_Sibling();
+ }
+
+ if ( CurrentChild != NULL ) {
+ CurrentParent = CurrentChild;
+ CurrentChild = CurrentParent->Get_Child();
+ }
+}
+
+
+void CProfileIterator::Enter_Parent( void )
+{
+ if ( CurrentParent->Get_Parent() != NULL ) {
+ CurrentParent = CurrentParent->Get_Parent();
+ }
+ CurrentChild = CurrentParent->Get_Child();
+}
+
+
+/***************************************************************************************************
+**
+** CProfileManager
+**
+***************************************************************************************************/
+
+CProfileNode CProfileManager::Root( "Root", NULL );
+CProfileNode * CProfileManager::CurrentNode = &CProfileManager::Root;
+int CProfileManager::FrameCounter = 0;
+unsigned long int CProfileManager::ResetTime = 0;
+
+
+/***********************************************************************************************
+ * CProfileManager::Start_Profile -- Begin a named profile *
+ * *
+ * Steps one level deeper into the tree, if a child already exists with the specified name *
+ * then it accumulates the profiling; otherwise a new child node is added to the profile tree. *
+ * *
+ * INPUT: *
+ * name - name of this profiling record *
+ * *
+ * WARNINGS: *
+ * The string used is assumed to be a static string; pointer compares are used throughout *
+ * the profiling code for efficiency. *
+ *=============================================================================================*/
+void CProfileManager::Start_Profile( const char * name )
+{
+ if (name != CurrentNode->Get_Name()) {
+ CurrentNode = CurrentNode->Get_Sub_Node( name );
+ }
+
+ CurrentNode->Call();
+}
+
+
+/***********************************************************************************************
+ * CProfileManager::Stop_Profile -- Stop timing and record the results. *
+ *=============================================================================================*/
+void CProfileManager::Stop_Profile( void )
+{
+ // Return will indicate whether we should back up to our parent (we may
+ // be profiling a recursive function)
+ if (CurrentNode->Return()) {
+ CurrentNode = CurrentNode->Get_Parent();
+ }
+}
+
+
+/***********************************************************************************************
+ * CProfileManager::Reset -- Reset the contents of the profiling system *
+ * *
+ * This resets everything except for the tree structure. All of the timing data is reset. *
+ *=============================================================================================*/
+void CProfileManager::Reset( void )
+{
+ gProfileClock.reset();
+ Root.Reset();
+ Root.Call();
+ FrameCounter = 0;
+ Profile_Get_Ticks(&ResetTime);
+}
+
+
+/***********************************************************************************************
+ * CProfileManager::Increment_Frame_Counter -- Increment the frame counter *
+ *=============================================================================================*/
+void CProfileManager::Increment_Frame_Counter( void )
+{
+ FrameCounter++;
+}
+
+
+/***********************************************************************************************
+ * CProfileManager::Get_Time_Since_Reset -- returns the elapsed time since last reset *
+ *=============================================================================================*/
+float CProfileManager::Get_Time_Since_Reset( void )
+{
+ unsigned long int time;
+ Profile_Get_Ticks(&time);
+ time -= ResetTime;
+ return (float)time / Profile_Get_Tick_Rate();
+}
+
+#include <stdio.h>
+
+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;i<spacing;i++) printf(".");
+ printf("----------------------------------\n");
+ for (i=0;i<spacing;i++) printf(".");
+ printf("Profiling: %s (total running time: %.3f ms) ---\n", profileIterator->Get_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;i<spacing;i++) printf(".");
+ }
+ printf("%d -- %s (%.2f %%) :: %.3f ms / frame (%d calls)\n",i, profileIterator->Get_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<spacing;i++) printf(".");
+ printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:",parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time);
+
+ for (i=0;i<numChildren;i++)
+ {
+ profileIterator->Enter_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
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btRaycastCallback.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btRaycastCallback.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btRaycastCallback.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btRaycastCallback.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,175 @@
+/*
+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 <stdio.h>
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+
+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.))
+{
+
+}
+
+
+
+void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+{
+ const btVector3 &vert0=triangle[0];
+ const btVector3 &vert1=triangle[1];
+ const btVector3 &vert2=triangle[2];
+
+ btVector3 v10; v10 = vert1 - vert0 ;
+ btVector3 v20; v20 = vert2 - vert0 ;
+
+ btVector3 triangleNormal; triangleNormal = v10.cross( v20 );
+
+ const btScalar dist = vert0.dot(triangleNormal);
+ btScalar dist_a = triangleNormal.dot(m_from) ;
+ dist_a-= dist;
+ btScalar dist_b = triangleNormal.dot(m_to);
+ dist_b -= dist;
+
+ if ( dist_a * dist_b >= btScalar(0.0) )
+ {
+ 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);
+ // Now we have the intersection point on the plane, we'll see if it's inside the triangle
+ // Add an epsilon as a tolerance for the raycast,
+ // in case the ray hits exacly on the edge of the triangle.
+ // It must be scaled for the triangle size.
+
+ if(distance < m_hitFraction)
+ {
+
+
+ btScalar edge_tolerance =triangleNormal.length2();
+ edge_tolerance *= btScalar(-0.0001);
+ btVector3 point; point.setInterpolate3( m_from, m_to, distance);
+ {
+ btVector3 v0p; v0p = vert0 - point;
+ btVector3 v1p; v1p = vert1 - point;
+ btVector3 cp0; cp0 = v0p.cross( v1p );
+
+ if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance)
+ {
+
+
+ btVector3 v2p; v2p = vert2 - point;
+ btVector3 cp1;
+ cp1 = v1p.cross( v2p);
+ if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance)
+ {
+ btVector3 cp2;
+ cp2 = v2p.cross(v0p);
+
+ if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance)
+ {
+ //@BP Mod
+ // Triangle normal isn't normalized
+ triangleNormal.normalize();
+
+ //@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);
+ }
+ else
+ {
+ m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+
+btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin)
+{
+ m_convexShape = convexShape;
+ m_convexShapeFrom = convexShapeFrom;
+ m_convexShapeTo = convexShapeTo;
+ m_triangleToWorld = triangleToWorld;
+ m_hitFraction = 1.0;
+ m_triangleCollisionMargin = triangleCollisionMargin;
+}
+
+void
+btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex)
+{
+ btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]);
+ triangleShape.setMargin(m_triangleCollisionMargin);
+
+ btVoronoiSimplexSolver simplexSolver;
+ btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
+
+//#define USE_SUBSIMPLEX_CONVEX_CAST 1
+//if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+ btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver);
+#else
+ //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver);
+ btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver);
+#endif //#USE_SUBSIMPLEX_CONVEX_CAST
+
+ btConvexCast::CastResult castResult;
+ castResult.m_fraction = btScalar(1.);
+ if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult))
+ {
+ //add hit
+ if (castResult.m_normal.length2() > btScalar(0.0001))
+ {
+ if (castResult.m_fraction < m_hitFraction)
+ {
+/* btContinuousConvexCast's normal is already in world space */
+/*
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+ //rotate normal into worldspace
+ castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal;
+#endif //USE_SUBSIMPLEX_CONVEX_CAST
+*/
+ castResult.m_normal.normalize();
+
+ reportHit (castResult.m_normal,
+ castResult.m_hitPoint,
+ castResult.m_fraction,
+ partId,
+ triangleIndex);
+ }
+ }
+ }
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btRaycastVehicle.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btRaycastVehicle.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btRaycastVehicle.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btRaycastVehicle.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,758 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+
+#include "LinearMath/btVector3.h"
+#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
+
+#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
+#include "LinearMath/btQuaternion.h"
+#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
+#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
+#include "BulletDynamics/Vehicle/btWheelInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+
+static btRigidBody s_fixedObject( 0,0,0);
+
+btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
+:m_vehicleRaycaster(raycaster),
+m_pitchControl(btScalar(0.))
+{
+ m_chassisBody = chassis;
+ m_indexRightAxis = 0;
+ m_indexUpAxis = 2;
+ m_indexForwardAxis = 1;
+ defaultInit(tuning);
+}
+
+
+void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
+{
+ (void)tuning;
+ m_currentVehicleSpeedKmHour = btScalar(0.);
+ m_steeringValue = btScalar(0.);
+
+}
+
+
+
+btRaycastVehicle::~btRaycastVehicle()
+{
+}
+
+
+//
+// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
+//
+btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
+{
+
+ btWheelInfoConstructionInfo ci;
+
+ ci.m_chassisConnectionCS = connectionPointCS;
+ ci.m_wheelDirectionCS = wheelDirectionCS0;
+ ci.m_wheelAxleCS = wheelAxleCS;
+ ci.m_suspensionRestLength = suspensionRestLength;
+ ci.m_wheelRadius = wheelRadius;
+ ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
+ ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
+ ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
+ ci.m_frictionSlip = tuning.m_frictionSlip;
+ ci.m_bIsFrontWheel = isFrontWheel;
+ ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
+
+ m_wheelInfo.push_back( btWheelInfo(ci));
+
+ btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];
+
+ updateWheelTransformsWS( wheel , false );
+ updateWheelTransform(getNumWheels()-1,false);
+ return wheel;
+}
+
+
+
+
+const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
+{
+ btAssert(wheelIndex < getNumWheels());
+ const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
+ return wheel.m_worldTransform;
+
+}
+
+void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
+{
+
+ btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
+ updateWheelTransformsWS(wheel,interpolatedTransform);
+ btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
+ const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
+ btVector3 fwd = up.cross(right);
+ fwd = fwd.normalize();
+// up = right.cross(fwd);
+// up.normalize();
+
+ //rotate around steering over de wheelAxleWS
+ btScalar steering = wheel.m_steering;
+
+ btQuaternion steeringOrn(up,steering);//wheel.m_steering);
+ btMatrix3x3 steeringMat(steeringOrn);
+
+ btQuaternion rotatingOrn(right,-wheel.m_rotation);
+ btMatrix3x3 rotatingMat(rotatingOrn);
+
+ btMatrix3x3 basis2(
+ right[0],fwd[0],up[0],
+ right[1],fwd[1],up[1],
+ right[2],fwd[2],up[2]
+ );
+
+ wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
+ wheel.m_worldTransform.setOrigin(
+ wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
+ );
+}
+
+void btRaycastVehicle::resetSuspension()
+{
+
+ int i;
+ for (i=0;i<m_wheelInfo.size(); i++)
+ {
+ btWheelInfo& wheel = m_wheelInfo[i];
+ wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
+ wheel.m_suspensionRelativeVelocity = btScalar(0.0);
+
+ wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
+ //wheel_info.setContactFriction(btScalar(0.0));
+ wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
+ }
+}
+
+void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
+{
+ wheel.m_raycastInfo.m_isInContact = false;
+
+ btTransform chassisTrans = getChassisWorldTransform();
+ if (interpolatedTransform && (getRigidBody()->getMotionState()))
+ {
+ getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
+ }
+
+ wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
+ wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ;
+ wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
+}
+
+btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
+{
+ updateWheelTransformsWS( wheel,false);
+
+
+ btScalar depth = -1;
+
+ btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;
+
+ btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
+ const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
+ wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
+ const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
+
+ btScalar param = btScalar(0.);
+
+ btVehicleRaycaster::btVehicleRaycasterResult rayResults;
+
+ btAssert(m_vehicleRaycaster);
+
+ void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
+
+ wheel.m_raycastInfo.m_groundObject = 0;
+
+ if (object)
+ {
+ param = rayResults.m_distFraction;
+ depth = raylen * rayResults.m_distFraction;
+ 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 = object;
+
+
+ btScalar hitDistance = param*raylen;
+ wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
+ //clamp on max suspension travel
+
+ btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
+ btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);
+ if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
+ {
+ wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
+ }
+ if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
+ {
+ wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
+ }
+
+ wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
+
+ btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );
+
+ btVector3 chassis_velocity_at_contactPoint;
+ btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();
+
+ chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
+
+ btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
+
+ if ( denominator >= btScalar(-0.1))
+ {
+ wheel.m_suspensionRelativeVelocity = btScalar(0.0);
+ wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
+ }
+ else
+ {
+ btScalar inv = btScalar(-1.) / denominator;
+ wheel.m_suspensionRelativeVelocity = projVel * inv;
+ wheel.m_clippedInvContactDotSuspension = inv;
+ }
+
+ } else
+ {
+ //put wheel info as in rest position
+ wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
+ wheel.m_suspensionRelativeVelocity = btScalar(0.0);
+ wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
+ wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
+ }
+
+ return depth;
+}
+
+
+const btTransform& btRaycastVehicle::getChassisWorldTransform() const
+{
+ /*if (getRigidBody()->getMotionState())
+ {
+ btTransform chassisWorldTrans;
+ getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
+ return chassisWorldTrans;
+ }
+ */
+
+
+ return getRigidBody()->getCenterOfMassTransform();
+}
+
+
+void btRaycastVehicle::updateVehicle( btScalar step )
+{
+ {
+ for (int i=0;i<getNumWheels();i++)
+ {
+ updateWheelTransform(i,false);
+ }
+ }
+
+
+ m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
+
+ const btTransform& chassisTrans = getChassisWorldTransform();
+
+ btVector3 forwardW (
+ chassisTrans.getBasis()[0][m_indexForwardAxis],
+ chassisTrans.getBasis()[1][m_indexForwardAxis],
+ chassisTrans.getBasis()[2][m_indexForwardAxis]);
+
+ if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
+ {
+ m_currentVehicleSpeedKmHour *= btScalar(-1.);
+ }
+
+ //
+ // simulate suspension
+ //
+
+ int i=0;
+ for (i=0;i<m_wheelInfo.size();i++)
+ {
+ btScalar depth;
+ depth = rayCast( m_wheelInfo[i]);
+ }
+
+ updateSuspension(step);
+
+
+ for (i=0;i<m_wheelInfo.size();i++)
+ {
+ //apply suspension force
+ btWheelInfo& wheel = m_wheelInfo[i];
+
+ btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
+
+ btScalar gMaxSuspensionForce = btScalar(6000.);
+ if (suspensionForce > gMaxSuspensionForce)
+ {
+ suspensionForce = gMaxSuspensionForce;
+ }
+ btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
+ btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
+
+ getRigidBody()->applyImpulse(impulse, relpos);
+
+ }
+
+
+
+ updateFriction( step);
+
+
+ for (i=0;i<m_wheelInfo.size();i++)
+ {
+ btWheelInfo& wheel = m_wheelInfo[i];
+ btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
+ btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
+
+ if (wheel.m_raycastInfo.m_isInContact)
+ {
+ const btTransform& chassisWorldTransform = getChassisWorldTransform();
+
+ btVector3 fwd (
+ chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
+ chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
+ chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
+
+ btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
+ fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
+
+ btScalar proj2 = fwd.dot(vel);
+
+ wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
+ wheel.m_rotation += wheel.m_deltaRotation;
+
+ } else
+ {
+ wheel.m_rotation += wheel.m_deltaRotation;
+ }
+
+ wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
+
+ }
+
+
+
+}
+
+
+void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
+{
+ btAssert(wheel>=0 && wheel < getNumWheels());
+
+ btWheelInfo& wheelInfo = getWheelInfo(wheel);
+ wheelInfo.m_steering = steering;
+}
+
+
+
+btScalar btRaycastVehicle::getSteeringValue(int wheel) const
+{
+ return getWheelInfo(wheel).m_steering;
+}
+
+
+void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
+{
+ btAssert(wheel>=0 && wheel < getNumWheels());
+ btWheelInfo& wheelInfo = getWheelInfo(wheel);
+ wheelInfo.m_engineForce = force;
+}
+
+
+const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const
+{
+ btAssert((index >= 0) && (index < getNumWheels()));
+
+ return m_wheelInfo[index];
+}
+
+btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
+{
+ btAssert((index >= 0) && (index < getNumWheels()));
+
+ return m_wheelInfo[index];
+}
+
+void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
+{
+ btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
+ getWheelInfo(wheelIndex).m_brake = brake;
+}
+
+
+void btRaycastVehicle::updateSuspension(btScalar deltaTime)
+{
+ (void)deltaTime;
+
+ btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
+
+ for (int w_it=0; w_it<getNumWheels(); w_it++)
+ {
+ btWheelInfo &wheel_info = m_wheelInfo[w_it];
+
+ if ( wheel_info.m_raycastInfo.m_isInContact )
+ {
+ btScalar force;
+ // Spring
+ {
+ btScalar susp_length = wheel_info.getSuspensionRestLength();
+ btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
+
+ btScalar length_diff = (susp_length - current_length);
+
+ force = wheel_info.m_suspensionStiffness
+ * length_diff * wheel_info.m_clippedInvContactDotSuspension;
+ }
+
+ // Damper
+ {
+ btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
+ {
+ btScalar susp_damping;
+ if ( projected_rel_vel < btScalar(0.0) )
+ {
+ susp_damping = wheel_info.m_wheelsDampingCompression;
+ }
+ else
+ {
+ susp_damping = wheel_info.m_wheelsDampingRelaxation;
+ }
+ force -= susp_damping * projected_rel_vel;
+ }
+ }
+
+ // RESULT
+ wheel_info.m_wheelsSuspensionForce = force * chassisMass;
+ if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
+ {
+ wheel_info.m_wheelsSuspensionForce = btScalar(0.);
+ }
+ }
+ else
+ {
+ wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
+ }
+ }
+
+}
+
+
+struct btWheelContactPoint
+{
+ btRigidBody* m_body0;
+ btRigidBody* m_body1;
+ btVector3 m_frictionPositionWorld;
+ btVector3 m_frictionDirectionWorld;
+ btScalar m_jacDiagABInv;
+ btScalar m_maxImpulse;
+
+
+ btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
+ :m_body0(body0),
+ m_body1(body1),
+ m_frictionPositionWorld(frictionPosWorld),
+ m_frictionDirectionWorld(frictionDirectionWorld),
+ m_maxImpulse(maxImpulse)
+ {
+ btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
+ btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
+ btScalar relaxation = 1.f;
+ m_jacDiagABInv = relaxation/(denom0+denom1);
+ }
+
+
+
+};
+
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
+{
+
+ btScalar j1=0.f;
+
+ const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
+
+ btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition();
+ btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
+
+ btScalar maxImpulse = contactPoint.m_maxImpulse;
+
+ btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
+ btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
+ btVector3 vel = vel1 - vel2;
+
+ btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
+
+ // calculate j that moves us to zero relative velocity
+ j1 = -vrel * contactPoint.m_jacDiagABInv;
+ btSetMin(j1, maxImpulse);
+ btSetMax(j1, -maxImpulse);
+
+ return j1;
+}
+
+
+
+
+btScalar sideFrictionStiffness2 = btScalar(1.0);
+void btRaycastVehicle::updateFriction(btScalar timeStep)
+{
+
+ //calculate the impulse, so that the wheels don't move sidewards
+ int numWheel = getNumWheels();
+ if (!numWheel)
+ return;
+
+ m_forwardWS.resize(numWheel);
+ m_axle.resize(numWheel);
+ m_forwardImpulse.resize(numWheel);
+ m_sideImpulse.resize(numWheel);
+
+ int numWheelsOnGround = 0;
+
+
+ //collapse all those loops into one!
+ for (int i=0;i<getNumWheels();i++)
+ {
+ btWheelInfo& wheelInfo = m_wheelInfo[i];
+ class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
+ if (groundObject)
+ numWheelsOnGround++;
+ m_sideImpulse[i] = btScalar(0.);
+ m_forwardImpulse[i] = btScalar(0.);
+
+ }
+
+ {
+
+ for (int i=0;i<getNumWheels();i++)
+ {
+
+ btWheelInfo& wheelInfo = m_wheelInfo[i];
+
+ class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
+
+ if (groundObject)
+ {
+
+ const btTransform& wheelTrans = getWheelTransformWS( i );
+
+ btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
+ m_axle[i] = btVector3(
+ wheelBasis0[0][m_indexRightAxis],
+ wheelBasis0[1][m_indexRightAxis],
+ wheelBasis0[2][m_indexRightAxis]);
+
+ const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
+ btScalar proj = m_axle[i].dot(surfNormalWS);
+ m_axle[i] -= surfNormalWS * proj;
+ m_axle[i] = m_axle[i].normalize();
+
+ m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
+ m_forwardWS[i].normalize();
+
+
+ resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
+ *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
+ btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
+
+ m_sideImpulse[i] *= sideFrictionStiffness2;
+
+ }
+
+
+ }
+ }
+
+ btScalar sideFactor = btScalar(1.);
+ btScalar fwdFactor = 0.5;
+
+ bool sliding = false;
+ {
+ for (int wheel =0;wheel <getNumWheels();wheel++)
+ {
+ btWheelInfo& wheelInfo = m_wheelInfo[wheel];
+ class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
+
+ btScalar rollingFriction = 0.f;
+
+ if (groundObject)
+ {
+ if (wheelInfo.m_engineForce != 0.f)
+ {
+ rollingFriction = wheelInfo.m_engineForce* timeStep;
+ } else
+ {
+ btScalar defaultRollingFrictionImpulse = 0.f;
+ btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
+ btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
+ rollingFriction = calcRollingFriction(contactPt);
+ }
+ }
+
+ //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
+
+
+
+
+ m_forwardImpulse[wheel] = btScalar(0.);
+ m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
+
+ if (groundObject)
+ {
+ m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
+
+ btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
+ btScalar maximpSide = maximp;
+
+ btScalar maximpSquared = maximp * maximpSide;
+
+
+ m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
+
+ btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
+ btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
+
+ btScalar impulseSquared = (x*x + y*y);
+
+ if (impulseSquared > maximpSquared)
+ {
+ sliding = true;
+
+ btScalar factor = maximp / btSqrt(impulseSquared);
+
+ m_wheelInfo[wheel].m_skidInfo *= factor;
+ }
+ }
+
+ }
+ }
+
+
+
+
+ if (sliding)
+ {
+ for (int wheel = 0;wheel < getNumWheels(); wheel++)
+ {
+ if (m_sideImpulse[wheel] != btScalar(0.))
+ {
+ if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
+ {
+ m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
+ m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
+ }
+ }
+ }
+ }
+
+ // apply the impulses
+ {
+ for (int wheel = 0;wheel<getNumWheels() ; wheel++)
+ {
+ btWheelInfo& wheelInfo = m_wheelInfo[wheel];
+
+ btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
+ m_chassisBody->getCenterOfMassPosition();
+
+ if (m_forwardImpulse[wheel] != btScalar(0.))
+ {
+ m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
+ }
+ if (m_sideImpulse[wheel] != btScalar(0.))
+ {
+ class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
+
+ btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
+ groundObject->getCenterOfMassPosition();
+
+
+ btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
+
+ rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
+ m_chassisBody->applyImpulse(sideImp,rel_pos);
+
+ //apply friction impulse on the ground
+ groundObject->applyImpulse(-sideImp,rel_pos2);
+ }
+ }
+ }
+
+
+}
+
+
+
+void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
+{
+
+ for (int v=0;v<this->getNumWheels();v++)
+ {
+ btVector3 wheelColor(0,255,255);
+ if (getWheelInfo(v).m_raycastInfo.m_isInContact)
+ {
+ wheelColor.setValue(0,0,255);
+ } else
+ {
+ wheelColor.setValue(255,0,255);
+ }
+
+ btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
+
+ btVector3 axle = btVector3(
+ getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
+ getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
+ getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
+
+ //debug wheels (cylinders)
+ debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
+ debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
+
+ }
+}
+
+
+void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
+{
+// RayResultCallback& resultCallback;
+
+ btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);
+
+ m_dynamicsWorld->rayTest(from, to, rayCallback);
+
+ if (rayCallback.hasHit())
+ {
+
+ btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
+ if (body && body->hasContactResponse())
+ {
+ result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
+ result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
+ result.m_hitNormalInWorld.normalize();
+ result.m_distFraction = rayCallback.m_closestHitFraction;
+ return body;
+ }
+ }
+ return 0;
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btRigidBody.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btRigidBody.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btRigidBody.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btRigidBody.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,317 @@
+/*
+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 "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btMotionState.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+//'temporarily' global variables
+btScalar gDeactivationTime = btScalar(2.);
+bool gDisableDeactivation = false;
+static int uniqueId = 0;
+
+
+btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
+{
+ setupRigidBody(constructionInfo);
+}
+
+btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
+{
+ btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
+ setupRigidBody(cinfo);
+}
+
+void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
+{
+
+ m_internalType=CO_RIGID_BODY;
+
+ m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+ m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ m_angularFactor.setValue(1,1,1);
+ m_linearFactor.setValue(1,1,1);
+ 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.);
+ m_angularDamping = btScalar(0.5);
+ m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
+ m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
+ m_optionalMotionState = constructionInfo.m_motionState;
+ m_contactSolverType = 0;
+ m_frictionSolverType = 0;
+ m_additionalDamping = constructionInfo.m_additionalDamping;
+ m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
+ m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
+ m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
+ m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
+
+ if (m_optionalMotionState)
+ {
+ m_optionalMotionState->getWorldTransform(m_worldTransform);
+ } else
+ {
+ m_worldTransform = constructionInfo.m_startWorldTransform;
+ }
+
+ m_interpolationWorldTransform = m_worldTransform;
+ m_interpolationLinearVelocity.setValue(0,0,0);
+ m_interpolationAngularVelocity.setValue(0,0,0);
+
+ //moved to btCollisionObject
+ m_friction = constructionInfo.m_friction;
+ m_restitution = constructionInfo.m_restitution;
+
+ setCollisionShape( constructionInfo.m_collisionShape );
+ m_debugBodyId = uniqueId++;
+
+ setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
+ setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
+ updateInertiaTensor();
+
+}
+
+
+void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform)
+{
+ btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
+}
+
+void btRigidBody::saveKinematicState(btScalar timeStep)
+{
+ //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
+ if (timeStep != btScalar(0.))
+ {
+ //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
+ if (getMotionState())
+ getMotionState()->getWorldTransform(m_worldTransform);
+ btVector3 linVel,angVel;
+
+ btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
+ m_interpolationLinearVelocity = m_linearVelocity;
+ m_interpolationAngularVelocity = m_angularVelocity;
+ m_interpolationWorldTransform = m_worldTransform;
+ //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
+ }
+}
+
+void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
+{
+ getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
+}
+
+
+
+
+void btRigidBody::setGravity(const btVector3& acceleration)
+{
+ if (m_inverseMass != btScalar(0.0))
+ {
+ m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
+ }
+ m_gravity_acceleration = acceleration;
+}
+
+
+
+
+
+
+void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
+{
+ m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+ m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+}
+
+
+
+
+///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)
+ {
+ //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+ //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+ if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
+ (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
+ {
+ m_angularVelocity *= m_additionalDampingFactor;
+ m_linearVelocity *= m_additionalDampingFactor;
+ }
+
+
+ btScalar speed = m_linearVelocity.length();
+ if (speed < m_linearDamping)
+ {
+ btScalar dampVel = btScalar(0.005);
+ if (speed > dampVel)
+ {
+ btVector3 dir = m_linearVelocity.normalized();
+ m_linearVelocity -= dir * dampVel;
+ } else
+ {
+ m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ }
+ }
+
+ btScalar angSpeed = m_angularVelocity.length();
+ if (angSpeed < m_angularDamping)
+ {
+ btScalar angDampVel = btScalar(0.005);
+ if (angSpeed > angDampVel)
+ {
+ btVector3 dir = m_angularVelocity.normalized();
+ m_angularVelocity -= dir * angDampVel;
+ } else
+ {
+ m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ }
+ }
+ }
+}
+
+
+void btRigidBody::applyGravity()
+{
+ if (isStaticOrKinematicObject())
+ return;
+
+ applyCentralForce(m_gravity);
+
+}
+
+void btRigidBody::proceedToTransform(const btTransform& newTrans)
+{
+ setCenterOfMassTransform( newTrans );
+}
+
+
+void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
+{
+ if (mass == btScalar(0.))
+ {
+ m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
+ m_inverseMass = btScalar(0.);
+ } else
+ {
+ m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
+ m_inverseMass = btScalar(1.0) / mass;
+ }
+
+ m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
+ inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
+ inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
+
+}
+
+
+
+void btRigidBody::updateInertiaTensor()
+{
+ m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
+}
+
+
+void btRigidBody::integrateVelocities(btScalar step)
+{
+ if (isStaticOrKinematicObject())
+ return;
+
+ m_linearVelocity += m_totalForce * (m_inverseMass * step);
+ m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
+
+#define MAX_ANGVEL SIMD_HALF_PI
+ /// clamp angular velocity. collision calculations will fail on higher angular velocities
+ btScalar angvel = m_angularVelocity.length();
+ if (angvel*step > MAX_ANGVEL)
+ {
+ m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
+ }
+
+}
+
+btQuaternion btRigidBody::getOrientation() const
+{
+ btQuaternion orn;
+ m_worldTransform.getBasis().getRotation(orn);
+ return orn;
+}
+
+
+void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
+{
+
+ if (isStaticOrKinematicObject())
+ {
+ m_interpolationWorldTransform = m_worldTransform;
+ } else
+ {
+ m_interpolationWorldTransform = xform;
+ }
+ m_interpolationLinearVelocity = getLinearVelocity();
+ m_interpolationAngularVelocity = getAngularVelocity();
+ m_worldTransform = xform;
+ updateInertiaTensor();
+}
+
+
+bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
+{
+ btRigidBody* otherRb = btRigidBody::upcast(co);
+ if (!otherRb)
+ return true;
+
+ for (int i = 0; i < m_constraintRefs.size(); ++i)
+ {
+ btTypedConstraint* c = m_constraintRefs[i];
+ if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
+ return false;
+ }
+
+ return true;
+}
+
+void btRigidBody::addConstraintRef(btTypedConstraint* c)
+{
+ int index = m_constraintRefs.findLinearSearch(c);
+ if (index == m_constraintRefs.size())
+ m_constraintRefs.push_back(c);
+
+ m_checkCollideWith = true;
+}
+
+void btRigidBody::removeConstraintRef(btTypedConstraint* c)
+{
+ m_constraintRefs.remove(c);
+ m_checkCollideWith = m_constraintRefs.size() > 0;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btScaledBvhTriangleMeshShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btScaledBvhTriangleMeshShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btScaledBvhTriangleMeshShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btScaledBvhTriangleMeshShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,121 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+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 "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
+
+btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling)
+:m_localScaling(localScaling),m_bvhTriMeshShape(childShape)
+{
+ m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE;
+}
+
+btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape()
+{
+}
+
+
+class btScaledTriangleCallback : public btTriangleCallback
+{
+ btTriangleCallback* m_originalCallback;
+
+ btVector3 m_localScaling;
+
+public:
+
+ btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling)
+ :m_originalCallback(originalCallback),
+ m_localScaling(localScaling)
+ {
+ }
+
+ virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+ {
+ btVector3 newTriangle[3];
+ newTriangle[0] = triangle[0]*m_localScaling;
+ newTriangle[1] = triangle[1]*m_localScaling;
+ newTriangle[2] = triangle[2]*m_localScaling;
+ m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex);
+ }
+};
+
+void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+ btScaledTriangleCallback scaledCallback(callback,m_localScaling);
+
+ btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ());
+ btVector3 scaledAabbMin,scaledAabbMax;
+
+ ///support negative scaling
+ scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0];
+ scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1];
+ scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2];
+
+ scaledAabbMax[0] = m_localScaling.getX() <= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0];
+ scaledAabbMax[1] = m_localScaling.getY() <= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1];
+ scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2];
+
+
+ m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax);
+}
+
+
+void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin();
+ btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax();
+
+ btVector3 tmpLocalAabbMin = localAabbMin * m_localScaling;
+ btVector3 tmpLocalAabbMax = localAabbMax * m_localScaling;
+
+ localAabbMin[0] = (m_localScaling.getX() >= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0];
+ localAabbMin[1] = (m_localScaling.getY() >= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1];
+ localAabbMin[2] = (m_localScaling.getZ() >= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2];
+ localAabbMax[0] = (m_localScaling.getX() <= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0];
+ localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1];
+ localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2];
+
+ btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
+ btScalar margin = m_bvhTriMeshShape->getMargin();
+ localHalfExtents += btVector3(margin,margin,margin);
+ btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
+
+ btMatrix3x3 abs_b = trans.getBasis().absolute();
+
+ 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 btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+{
+ m_localScaling = scaling;
+}
+
+const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const
+{
+ return m_localScaling;
+}
+
+void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ ///don't make this a movable object!
+// btAssert(0);
+}
More information about the llvm-commits
mailing list